diff --git a/modules/bullet/btRayShape.cpp b/modules/bullet/btRayShape.cpp index b902d08eca7..b60d6ba693a 100644 --- a/modules/bullet/btRayShape.cpp +++ b/modules/bullet/btRayShape.cpp @@ -54,6 +54,11 @@ void btRayShape::setLength(btScalar p_length) { reload_cache(); } +void btRayShape::setMargin(btScalar margin) { + btConvexInternalShape::setMargin(margin); + reload_cache(); +} + void btRayShape::setSlipsOnSlope(bool p_slipsOnSlope) { slipsOnSlope = p_slipsOnSlope; @@ -77,10 +82,9 @@ void btRayShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVecto } void btRayShape::getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const { -#define MARGIN_BROADPHASE 0.1 btVector3 localAabbMin(0, 0, 0); - btVector3 localAabbMax(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin)); - btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax); + btVector3 localAabbMax(m_shapeAxis * m_cacheScaledLength); + btTransformAabb(localAabbMin, localAabbMax, m_collisionMargin, t, aabbMin, aabbMax); } void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const { @@ -100,5 +104,5 @@ void btRayShape::reload_cache() { m_cacheScaledLength = m_length * m_localScaling[2]; m_cacheSupportPoint.setIdentity(); - m_cacheSupportPoint.setOrigin(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin)); + m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength); } diff --git a/modules/bullet/btRayShape.h b/modules/bullet/btRayShape.h index 7fedb740831..7f3229b3e8e 100644 --- a/modules/bullet/btRayShape.h +++ b/modules/bullet/btRayShape.h @@ -60,6 +60,8 @@ public: void setLength(btScalar p_length); btScalar getLength() const { return m_length; } + virtual void setMargin(btScalar margin); + void setSlipsOnSlope(bool p_slipOnSlope); bool getSlipsOnSlope() const { return slipsOnSlope; } diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp index 3a90bdc6ae1..eb87901c248 100644 --- a/modules/bullet/collision_object_bullet.cpp +++ b/modules/bullet/collision_object_bullet.cpp @@ -43,7 +43,9 @@ @author AndreaCatania */ -#define enableDynamicAabbTree false +// We enable dynamic AABB tree so that we can actually perform a broadphase on bodies with compound collision shapes. +// This is crucial for the performance of kinematic bodies and for bodies with transforming shapes. +#define enableDynamicAabbTree true CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {} @@ -284,7 +286,6 @@ void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transfor ERR_FAIL_INDEX(p_index, get_shape_count()); shapes.write[p_index].set_transform(p_transform); - // Note, enableDynamicAabbTree is false because on transform change compound is destroyed reload_shapes(); } diff --git a/modules/bullet/godot_ray_world_algorithm.cpp b/modules/bullet/godot_ray_world_algorithm.cpp index 3e062394539..2ba75b9a98b 100644 --- a/modules/bullet/godot_ray_world_algorithm.cpp +++ b/modules/bullet/godot_ray_world_algorithm.cpp @@ -39,6 +39,9 @@ @author AndreaCatania */ +// Epsilon to account for floating point inaccuracies +#define RAY_PENETRATION_DEPTH_EPSILON 0.01 + GodotRayWorldAlgorithm::CreateFunc::CreateFunc(const btDiscreteDynamicsWorld *world) : m_world(world) {} @@ -100,8 +103,8 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1)); - if (depth >= -ray_shape->getMargin() * 0.5) - depth = 0; + if (depth > -RAY_PENETRATION_DEPTH_EPSILON) + depth = 0.0; if (ray_shape->getSlipsOnSlope()) resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth); diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 8fb8eba057f..6bfd98873ec 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -1043,23 +1043,16 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p btVector3 recover_motion(0, 0, 0); int rays_found = 0; + int rays_found_this_round = 0; for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) { - int last_ray_index = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max, recover_motion, r_results); + PhysicsServer::SeparationResult *next_results = &r_results[rays_found]; + rays_found_this_round = recover_from_penetration_ray(p_body, body_transform, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, p_result_max - rays_found, recover_motion, next_results); - rays_found = MAX(last_ray_index, rays_found); - if (!rays_found) { - break; - } else { + rays_found += rays_found_this_round; + if (rays_found_this_round == 0) { body_transform.getOrigin() += recover_motion; - } - } - - //optimize results (remove non colliding) - for (int i = 0; i < rays_found; i++) { - if (r_results[i].collision_depth >= 0) { - rays_found--; - SWAP(r_results[i], r_results[rays_found]); + break; } } @@ -1069,18 +1062,47 @@ int SpaceBullet::test_ray_separation(RigidBodyBullet *p_body, const Transform &p struct RecoverPenetrationBroadPhaseCallback : public btBroadphaseAabbCallback { private: + btDbvtVolume bounds; + const btCollisionObject *self_collision_object; uint32_t collision_layer; uint32_t collision_mask; -public: - Vector result_collision_objects; + struct CompoundLeafCallback : btDbvt::ICollide { + private: + RecoverPenetrationBroadPhaseCallback *parent_callback; + btCollisionObject *collision_object; + + public: + CompoundLeafCallback(RecoverPenetrationBroadPhaseCallback *p_parent_callback, btCollisionObject *p_collision_object) : + parent_callback(p_parent_callback), + collision_object(p_collision_object) { + } + + void Process(const btDbvtNode *leaf) { + BroadphaseResult result; + result.collision_object = collision_object; + result.compound_child_index = leaf->dataAsInt; + parent_callback->results.push_back(result); + } + }; public: - RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask) : + struct BroadphaseResult { + btCollisionObject *collision_object; + int compound_child_index; + }; + + Vector results; + +public: + RecoverPenetrationBroadPhaseCallback(const btCollisionObject *p_self_collision_object, uint32_t p_collision_layer, uint32_t p_collision_mask, btVector3 p_aabb_min, btVector3 p_aabb_max) : self_collision_object(p_self_collision_object), collision_layer(p_collision_layer), - collision_mask(p_collision_mask) {} + collision_mask(p_collision_mask) { + + bounds = btDbvtVolume::FromMM(p_aabb_min, p_aabb_max); + } virtual ~RecoverPenetrationBroadPhaseCallback() {} @@ -1089,35 +1111,53 @@ public: btCollisionObject *co = static_cast(proxy->m_clientObject); if (co->getInternalType() <= btCollisionObject::CO_RIGID_BODY) { if (self_collision_object != proxy->m_clientObject && GodotFilterCallback::test_collision_filters(collision_layer, collision_mask, proxy->m_collisionFilterGroup, proxy->m_collisionFilterMask)) { - result_collision_objects.push_back(co); + if (co->getCollisionShape()->isCompound()) { + const btCompoundShape *cs = static_cast(co->getCollisionShape()); + + if (cs->getNumChildShapes() > 1) { + const btDbvt *tree = cs->getDynamicAabbTree(); + ERR_FAIL_COND_V(tree == NULL, true); + + // Transform bounds into compound shape local space + const btTransform other_in_compound_space = co->getWorldTransform().inverse(); + const btMatrix3x3 abs_b = other_in_compound_space.getBasis().absolute(); + const btVector3 local_center = other_in_compound_space(bounds.Center()); + const btVector3 local_extent = bounds.Extents().dot3(abs_b[0], abs_b[1], abs_b[2]); + const btVector3 local_aabb_min = local_center - local_extent; + const btVector3 local_aabb_max = local_center + local_extent; + const btDbvtVolume local_bounds = btDbvtVolume::FromMM(local_aabb_min, local_aabb_max); + + // Test collision against compound child shapes using its AABB tree + CompoundLeafCallback compound_leaf_callback(this, co); + tree->collideTV(tree->m_root, local_bounds, compound_leaf_callback); + } else { + // If there's only a single child shape then there's no need to search any more, we know which child overlaps + BroadphaseResult result; + result.collision_object = co; + result.compound_child_index = 0; + results.push_back(result); + } + } else { + BroadphaseResult result; + result.collision_object = co; + result.compound_child_index = -1; + results.push_back(result); + } return true; } } return false; } - - void reset() { - result_collision_objects.clear(); - } }; bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result) { - RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask()); + // Calculate the cummulative AABB of all shapes of the kinematic body + btVector3 aabb_min, aabb_max; + bool shapes_found = false; - btTransform body_shape_position; - btTransform body_shape_position_recovered; - - // Broad phase support - btVector3 minAabb, maxAabb; - - bool penetration = false; - - // For each shape for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { - recover_broad_result.reset(); - const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]); if (!kin_shape.is_active()) { continue; @@ -1128,15 +1168,56 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran continue; } - body_shape_position = p_body_position * kin_shape.transform; - body_shape_position_recovered = body_shape_position; - body_shape_position_recovered.getOrigin() += r_delta_recover_movement; + btTransform shape_transform = p_body_position * kin_shape.transform; + shape_transform.getOrigin() += r_delta_recover_movement; - kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb); - dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result); + btVector3 shape_aabb_min, shape_aabb_max; + kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max); - for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) { - btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i]; + if (!shapes_found) { + aabb_min = shape_aabb_min; + aabb_max = shape_aabb_max; + shapes_found = true; + } else { + aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x()); + aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y()); + aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z()); + + aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x()); + aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y()); + aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z()); + } + } + + // If there are no shapes then there is no penetration either + if (!shapes_found) { + return false; + } + + // Perform broadphase test + RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max); + dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result); + + bool penetration = false; + + // Perform narrowphase per shape + for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { + + const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]); + if (!kin_shape.is_active()) { + continue; + } + + if (kin_shape.shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { + // Skip rayshape in order to implement custom separation process + continue; + } + + btTransform shape_transform = p_body_position * kin_shape.transform; + shape_transform.getOrigin() += r_delta_recover_movement; + + for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) { + btCollisionObject *otherObject = recover_broad_result.results[i].collision_object; if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) { otherObject->activate(); // Force activation of hitten rigid, soft body continue; @@ -1144,30 +1225,28 @@ bool SpaceBullet::recover_from_penetration(RigidBodyBullet *p_body, const btTran continue; if (otherObject->getCollisionShape()->isCompound()) { + const btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); + int shape_idx = recover_broad_result.results[i].compound_child_index; + ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false); - // Each convex shape - btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); - for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) { + if (cs->getChildShape(shape_idx)->isConvex()) { + if (RFP_convex_convex_test(kin_shape.shape, static_cast(cs->getChildShape(shape_idx)), otherObject, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { - if (cs->getChildShape(x)->isConvex()) { - if (RFP_convex_convex_test(kin_shape.shape, static_cast(cs->getChildShape(x)), otherObject, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { + penetration = true; + } + } else { + if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { - penetration = true; - } - } else { - if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { - - penetration = true; - } + penetration = true; } } } else if (otherObject->getCollisionShape()->isConvex()) { /// Execute GJK test against object shape - if (RFP_convex_convex_test(kin_shape.shape, static_cast(otherObject->getCollisionShape()), otherObject, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { + if (RFP_convex_convex_test(kin_shape.shape, static_cast(otherObject->getCollisionShape()), otherObject, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { penetration = true; } } else { - if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { + if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, r_recover_result)) { penetration = true; } @@ -1183,7 +1262,6 @@ bool SpaceBullet::RFP_convex_convex_test(const btConvexShape *p_shapeA, const bt // Initialize GJK input btGjkPairDetector::ClosestPointInput gjk_input; gjk_input.m_transformA = p_transformA; - gjk_input.m_transformA.getOrigin() += r_delta_recover_movement; gjk_input.m_transformB = p_transformB; // Perform GJK test @@ -1214,7 +1292,6 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC /// Contact test btTransform tA(p_transformA); - tA.getOrigin() += r_delta_recover_movement; btCollisionObjectWrapper obA(NULL, p_shapeA, p_objectA, tA, -1, p_shapeId_A); btCollisionObjectWrapper obB(NULL, p_shapeB, p_objectB, p_transformB, -1, p_shapeId_B); @@ -1246,39 +1323,81 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC return false; } -void SpaceBullet::convert_to_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const { +int SpaceBullet::add_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const { - const btRigidBody *btRigid = static_cast(p_other_object); - CollisionObjectBullet *collisionObject = static_cast(p_other_object->getUserPointer()); + // optimize results (ignore non-colliding) + if (p_recover_result.penetration_distance < 0.0) { + const btRigidBody *btRigid = static_cast(p_other_object); + CollisionObjectBullet *collisionObject = static_cast(p_other_object->getUserPointer()); - r_result->collision_depth = p_recover_result.penetration_distance; - B_TO_G(p_recover_result.pointWorld, r_result->collision_point); - B_TO_G(p_recover_result.normal, r_result->collision_normal); - B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); - r_result->collision_local_shape = p_shape_id; - r_result->collider_id = collisionObject->get_instance_id(); - r_result->collider = collisionObject->get_self(); - r_result->collider_shape = p_recover_result.other_compound_shape_index; + r_result->collision_depth = p_recover_result.penetration_distance; + B_TO_G(p_recover_result.pointWorld, r_result->collision_point); + B_TO_G(p_recover_result.normal, r_result->collision_normal); + B_TO_G(btRigid->getVelocityInLocalPoint(p_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_result->collider_velocity); + r_result->collision_local_shape = p_shape_id; + r_result->collider_id = collisionObject->get_instance_id(); + r_result->collider = collisionObject->get_self(); + r_result->collider_shape = p_recover_result.other_compound_shape_index; + + return 1; + } else { + return 0; + } } int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results) { - RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask()); + // Calculate the cummulative AABB of all shapes of the kinematic body + btVector3 aabb_min, aabb_max; + bool shapes_found = false; - btTransform body_shape_position; - btTransform body_shape_position_recovered; - - // Broad phase support - btVector3 minAabb, maxAabb; - - int ray_index = 0; - - // For each shape for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { - recover_broad_result.reset(); + const RigidBodyBullet::KinematicShape &kin_shape(p_body->get_kinematic_utilities()->shapes[kinIndex]); + if (!kin_shape.is_active()) { + continue; + } - if (ray_index >= p_result_max) { + if (kin_shape.shape->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE) { + continue; + } + + btTransform shape_transform = p_body_position * kin_shape.transform; + shape_transform.getOrigin() += r_delta_recover_movement; + + btVector3 shape_aabb_min, shape_aabb_max; + kin_shape.shape->getAabb(shape_transform, shape_aabb_min, shape_aabb_max); + + if (!shapes_found) { + aabb_min = shape_aabb_min; + aabb_max = shape_aabb_max; + shapes_found = true; + } else { + aabb_min.setX((aabb_min.x() < shape_aabb_min.x()) ? aabb_min.x() : shape_aabb_min.x()); + aabb_min.setY((aabb_min.y() < shape_aabb_min.y()) ? aabb_min.y() : shape_aabb_min.y()); + aabb_min.setZ((aabb_min.z() < shape_aabb_min.z()) ? aabb_min.z() : shape_aabb_min.z()); + + aabb_max.setX((aabb_max.x() > shape_aabb_max.x()) ? aabb_max.x() : shape_aabb_max.x()); + aabb_max.setY((aabb_max.y() > shape_aabb_max.y()) ? aabb_max.y() : shape_aabb_max.y()); + aabb_max.setZ((aabb_max.z() > shape_aabb_max.z()) ? aabb_max.z() : shape_aabb_max.z()); + } + } + + // If there are no shapes then there is no penetration either + if (!shapes_found) { + return 0; + } + + // Perform broadphase test + RecoverPenetrationBroadPhaseCallback recover_broad_result(p_body->get_bt_collision_object(), p_body->get_collision_layer(), p_body->get_collision_mask(), aabb_min, aabb_max); + dynamicsWorld->getBroadphase()->aabbTest(aabb_min, aabb_max, recover_broad_result); + + int ray_count = 0; + + // Perform narrowphase per shape + for (int kinIndex = p_body->get_kinematic_utilities()->shapes.size() - 1; 0 <= kinIndex; --kinIndex) { + + if (ray_count >= p_result_max) { break; } @@ -1291,15 +1410,11 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT continue; } - body_shape_position = p_body_position * kin_shape.transform; - body_shape_position_recovered = body_shape_position; - body_shape_position_recovered.getOrigin() += r_delta_recover_movement; + btTransform shape_transform = p_body_position * kin_shape.transform; + shape_transform.getOrigin() += r_delta_recover_movement; - kin_shape.shape->getAabb(body_shape_position_recovered, minAabb, maxAabb); - dynamicsWorld->getBroadphase()->aabbTest(minAabb, maxAabb, recover_broad_result); - - for (int i = recover_broad_result.result_collision_objects.size() - 1; 0 <= i; --i) { - btCollisionObject *otherObject = recover_broad_result.result_collision_objects[i]; + for (int i = recover_broad_result.results.size() - 1; 0 <= i; --i) { + btCollisionObject *otherObject = recover_broad_result.results[i].collision_object; if (p_infinite_inertia && !otherObject->isStaticOrKinematicObject()) { otherObject->activate(); // Force activation of hitten rigid, soft body continue; @@ -1307,29 +1422,25 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT continue; if (otherObject->getCollisionShape()->isCompound()) { + const btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); + int shape_idx = recover_broad_result.results[i].compound_child_index; + ERR_FAIL_COND_V(shape_idx < 0 || shape_idx >= cs->getNumChildShapes(), false); - // Each convex shape - btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); - for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) { + RecoverResult recover_result; + if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(shape_idx), p_body->get_bt_collision_object(), otherObject, kinIndex, shape_idx, shape_transform, otherObject->getWorldTransform() * cs->getChildTransform(shape_idx), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { - RecoverResult recover_result; - if (RFP_convex_world_test(kin_shape.shape, cs->getChildShape(x), p_body->get_bt_collision_object(), otherObject, kinIndex, x, body_shape_position, otherObject->getWorldTransform() * cs->getChildTransform(x), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { - - convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject); - } + ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject); } } else { RecoverResult recover_result; - if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, body_shape_position, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { + if (RFP_convex_world_test(kin_shape.shape, otherObject->getCollisionShape(), p_body->get_bt_collision_object(), otherObject, kinIndex, 0, shape_transform, otherObject->getWorldTransform(), p_recover_movement_scale, r_delta_recover_movement, &recover_result)) { - convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject); + ray_count = add_separation_result(&r_results[ray_count], recover_result, kinIndex, otherObject); } } } - - ++ray_index; } - return ray_index; + return ray_count; } diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index 7bf6a216b58..6b3d65edf6f 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -212,7 +212,7 @@ private: /// Using this we leave Bullet to select the best algorithm, For example GJK in case we have Convex Convex, or a Bullet accelerated algorithm bool RFP_convex_world_test(const btConvexShape *p_shapeA, const btCollisionShape *p_shapeB, btCollisionObject *p_objectA, btCollisionObject *p_objectB, int p_shapeId_A, int p_shapeId_B, const btTransform &p_transformA, const btTransform &p_transformB, btScalar p_recover_movement_scale, btVector3 &r_delta_recover_movement, RecoverResult *r_recover_result = NULL); - void convert_to_separation_result(PhysicsServer::SeparationResult *r_result, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const; + int add_separation_result(PhysicsServer::SeparationResult *r_results, const SpaceBullet::RecoverResult &p_recover_result, int p_shape_id, const btCollisionObject *p_other_object) const; int recover_from_penetration_ray(RigidBodyBullet *p_body, const btTransform &p_body_position, btScalar p_recover_movement_scale, bool p_infinite_inertia, int p_result_max, btVector3 &r_delta_recover_movement, PhysicsServer::SeparationResult *r_results); }; #endif diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp index e2dc89aa6e2..57af9511109 100644 --- a/scene/3d/physics_body.cpp +++ b/scene/3d/physics_body.cpp @@ -1181,19 +1181,16 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve while (p_max_slides) { Collision collision; - bool found_collision = false; - int test_type = 0; - - do { + for (int i = 0; i < 2; ++i) { bool collided; - if (test_type == 0) { //collide + if (i == 0) { //collide collided = move_and_collide(motion, p_infinite_inertia, collision); if (!collided) { motion = Vector3(); //clear because no collision happened and motion completed } - } else { + } else { //separate raycasts (if any) collided = separate_raycast_shapes(p_infinite_inertia, collision); if (collided) { collision.remainder = motion; //keep @@ -1219,7 +1216,7 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve floor_velocity = collision.collider_vel; if (p_stop_on_slope) { - if ((lv_n + p_floor_direction).length() < 0.01) { + if ((lv_n + p_floor_direction).length() < 0.01 && collision.travel.length() < 1) { Transform gt = get_global_transform(); gt.origin -= collision.travel; set_global_transform(gt); @@ -1240,21 +1237,18 @@ Vector3 KinematicBody::move_and_slide(const Vector3 &p_linear_velocity, const Ve motion = motion.slide(p_floor_direction); lv = lv.slide(p_floor_direction); } else { - Vector3 n = collision.normal; motion = motion.slide(n); lv = lv.slide(n); } - for (int i = 0; i < 3; i++) { - if (locked_axis & (1 << i)) { - lv[i] = 0; + for (int j = 0; j < 3; j++) { + if (locked_axis & (1 << j)) { + lv[j] = 0; } } } - - ++test_type; - } while (!p_stop_on_slope && test_type < 2); + } if (!found_collision || motion == Vector3()) break; diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h index 589af980625..aa6030d44e4 100644 --- a/scene/3d/physics_body.h +++ b/scene/3d/physics_body.h @@ -317,7 +317,7 @@ protected: static void _bind_methods(); public: - bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collisionz, bool p_exclude_raycast_shapes = true, bool p_test_only = false); + bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false); bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia); bool separate_raycast_shapes(bool p_infinite_inertia, Collision &r_collision); diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp index e52cc376c0c..8b9f2108508 100644 --- a/servers/physics/space_sw.cpp +++ b/servers/physics/space_sw.cpp @@ -630,7 +630,7 @@ int SpaceSW::test_body_ray_separation(BodySW *p_body, const Transform &p_transfo int ray_index = -1; //reuse shape for (int k = 0; k < rays_found; k++) { - if (r_results[ray_index].collision_local_shape == j) { + if (r_results[k].collision_local_shape == j) { ray_index = k; } }