diff options
| -rw-r--r-- | modules/bullet/godot_result_callbacks.cpp | 23 | ||||
| -rw-r--r-- | modules/bullet/godot_result_callbacks.h | 7 | ||||
| -rw-r--r-- | modules/bullet/space_bullet.cpp | 15 |
3 files changed, 17 insertions, 28 deletions
diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp index 7c051f8f17..72c982bb0b 100644 --- a/modules/bullet/godot_result_callbacks.cpp +++ b/modules/bullet/godot_result_callbacks.cpp @@ -257,18 +257,15 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth) { - if (depth < 0) { - // Has penetration - if (m_most_penetrated_distance > depth) { - - bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); - - m_most_penetrated_distance = depth; - m_pointCollisionObject = (isSwapped ? m_body0Wrap : m_body1Wrap)->getCollisionObject(); - m_other_compound_shape_index = isSwapped ? m_index1 : m_index0; - m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; - m_pointWorld = isSwapped ? (pointInWorldOnB + normalOnBInWorld * depth) : pointInWorldOnB; - m_penetration_distance = depth; - } + // Has penetration + if (m_penetration_distance < ABS(depth)) { + + bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + + m_penetration_distance = depth; + m_pointCollisionObject = (isSwapped ? m_body0Wrap : m_body1Wrap)->getCollisionObject(); + m_other_compound_shape_index = isSwapped ? m_index1 : m_index0; + m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; + m_pointWorld = isSwapped ? (pointInWorldOnB + normalOnBInWorld * depth) : pointInWorldOnB; } } diff --git a/modules/bullet/godot_result_callbacks.h b/modules/bullet/godot_result_callbacks.h index ed6d4b7d6d..60493d4788 100644 --- a/modules/bullet/godot_result_callbacks.h +++ b/modules/bullet/godot_result_callbacks.h @@ -187,18 +187,15 @@ struct GodotDeepPenetrationContactResultCallback : public btManifoldResult { int m_other_compound_shape_index; const btCollisionObject *m_pointCollisionObject; - btScalar m_most_penetrated_distance; - GodotDeepPenetrationContactResultCallback(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) : btManifoldResult(body0Wrap, body1Wrap), m_pointCollisionObject(NULL), m_penetration_distance(0), - m_other_compound_shape_index(0), - m_most_penetrated_distance(1e20) {} + m_other_compound_shape_index(0) {} void reset() { m_pointCollisionObject = NULL; - m_most_penetrated_distance = 1e20; + m_penetration_distance = 0; } bool hasHit() { diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index e20a79f667..8c15758e0f 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -790,7 +790,7 @@ void SpaceBullet::update_gravity() { /// I'm leaving this here just for future tests. /// Debug motion and normal vector drawing #define debug_test_motion 0 -#define PERFORM_INITIAL_UNSTACK 0 + #define RECOVERING_MOVEMENT_SCALE 0.4 #define RECOVERING_MOVEMENT_CYCLES 4 @@ -842,7 +842,6 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f G_TO_B(p_from, body_safe_position); UNSCALE_BT_BASIS(body_safe_position); -#if PERFORM_INITIAL_UNSTACK btVector3 recover_initial_position(0, 0, 0); { /// Phase one - multi shapes depenetration using margin for (int t(RECOVERING_MOVEMENT_CYCLES); 0 < t; --t) { @@ -854,7 +853,6 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f // Add recover movement in order to make it safe body_safe_position.getOrigin() += recover_initial_position; } -#endif btVector3 motion; G_TO_B(p_motion, motion); @@ -893,7 +891,7 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f btResult.m_collisionFilterGroup = p_body->get_collision_layer(); btResult.m_collisionFilterMask = p_body->get_collision_mask(); - dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, 0.002); + dynamicsWorld->convexSweepTest(convex_shape_test, shape_world_from, shape_world_to, btResult, dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration); if (btResult.hasHit()) { /// Since for each sweep test I fix the motion of new shapes in base the recover result, @@ -918,11 +916,8 @@ bool SpaceBullet::test_body_motion(RigidBodyBullet *p_body, const Transform &p_f l_has_penetration = recover_from_penetration(p_body, body_safe_position, RECOVERING_MOVEMENT_SCALE, p_infinite_inertia, delta_recover_movement, &r_recover_result); if (r_result) { -#if PERFORM_INITIAL_UNSTACK B_TO_G(motion + delta_recover_movement + recover_initial_position, r_result->motion); -#else - B_TO_G(motion + delta_recover_movement, r_result->motion); -#endif + if (l_has_penetration) { has_penetration = true; if (l_penetration_distance <= r_recover_result.penetration_distance) { @@ -1112,7 +1107,7 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC 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); - btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CLOSEST_POINT_ALGORITHMS); + btCollisionAlgorithm *algorithm = dispatcher->findAlgorithm(&obA, &obB, NULL, BT_CONTACT_POINT_ALGORITHMS); if (algorithm) { GodotDeepPenetrationContactResultCallback contactPointResult(&obA, &obB); //discrete collision detection query @@ -1122,7 +1117,7 @@ bool SpaceBullet::RFP_convex_world_test(const btConvexShape *p_shapeA, const btC dispatcher->freeCollisionAlgorithm(algorithm); if (contactPointResult.hasHit()) { - r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale); + r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * p_recover_movement_scale); if (r_recover_result) { if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) { |