From f7511511b12977d0385198ea0037ef4c62e75d47 Mon Sep 17 00:00:00 2001 From: Daniel Rakos Date: Mon, 11 Feb 2019 01:48:33 +0100 Subject: [PATCH] Fix RayShape collision when used with a KinematicBody (Bullet Physics) - Added code handling non-compound collision to recover_from_penetration_ray() which is now needed due to the optimization avoiding the use of compound collisions when only a single collision shape is used. - Removed arbitrary margin applied in the collision algorithm of RayShapes which causes jittered movement. For lack of a better replacement and for lack of any explanation on why it has been introduced, it's now using the shape's margin property instead which is small enough to not show visible jitter. - Tried to get rid of inconsistent uses of the collision margin. - Removed hack from GodotDeepPenetrationContactResultCallback::addContactPoint for RayShape collision as it's no longer needed as the collision algorithm of RayShapes correctly calculates the contact normal for a while now. Fixes #25227. --- modules/bullet/btRayShape.cpp | 6 ++-- modules/bullet/godot_ray_world_algorithm.cpp | 6 ++-- modules/bullet/godot_result_callbacks.cpp | 10 +----- modules/bullet/space_bullet.cpp | 38 +++++++++++++------- modules/bullet/space_bullet.h | 1 + 5 files changed, 32 insertions(+), 29 deletions(-) diff --git a/modules/bullet/btRayShape.cpp b/modules/bullet/btRayShape.cpp index 935d86daa66..b902d08eca7 100644 --- a/modules/bullet/btRayShape.cpp +++ b/modules/bullet/btRayShape.cpp @@ -79,7 +79,7 @@ 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_length); + btVector3 localAabbMax(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin)); btTransformAabb(localAabbMin, localAabbMax, MARGIN_BROADPHASE, t, aabbMin, aabbMax); } @@ -97,8 +97,8 @@ void btRayShape::getPreferredPenetrationDirection(int index, btVector3 &penetrat void btRayShape::reload_cache() { - m_cacheScaledLength = m_length * m_localScaling[2] + m_collisionMargin; + m_cacheScaledLength = m_length * m_localScaling[2]; m_cacheSupportPoint.setIdentity(); - m_cacheSupportPoint.setOrigin(m_shapeAxis * m_cacheScaledLength); + m_cacheSupportPoint.setOrigin(m_shapeAxis * (m_cacheScaledLength + m_collisionMargin)); } diff --git a/modules/bullet/godot_ray_world_algorithm.cpp b/modules/bullet/godot_ray_world_algorithm.cpp index 449f625e177..cadc8dd59ef 100644 --- a/modules/bullet/godot_ray_world_algorithm.cpp +++ b/modules/bullet/godot_ray_world_algorithm.cpp @@ -35,8 +35,6 @@ #include -#define RAY_STABILITY_MARGIN 0.1 - /** @author AndreaCatania */ @@ -102,8 +100,8 @@ void GodotRayWorldAlgorithm::processCollision(const btCollisionObjectWrapper *bo btScalar depth(ray_shape->getScaledLength() * (btResult.m_closestHitFraction - 1)); - if (depth >= -RAY_STABILITY_MARGIN) - depth = 0; + if (depth >= -ray_shape->getMargin()) + depth *= 0.5; if (ray_shape->getSlipsOnSlope()) resultOut->addContactPoint(btResult.m_hitNormalWorld, btResult.m_hitPointWorld, depth); diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp index 7babfcc1335..360950c4c75 100644 --- a/modules/bullet/godot_result_callbacks.cpp +++ b/modules/bullet/godot_result_callbacks.cpp @@ -333,14 +333,6 @@ void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 m_other_compound_shape_index = isSwapped ? m_index0 : m_index1; m_pointWorld = isSwapped ? (pointInWorldOnB + (normalOnBInWorld * depth)) : pointInWorldOnB; - const btCollisionObjectWrapper *bw0 = m_body0Wrap; - if (isSwapped) - bw0 = m_body1Wrap; - - if (bw0->getCollisionShape()->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) { - m_pointNormalWorld = bw0->m_worldTransform.getBasis().transpose() * btVector3(0, 0, 1); - } else { - m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; - } + m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld; } } diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp index 6562b18b3c2..40db5f8a236 100644 --- a/modules/bullet/space_bullet.cpp +++ b/modules/bullet/space_bullet.cpp @@ -1224,6 +1224,21 @@ 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 { + + 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; +} + 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()); @@ -1275,22 +1290,19 @@ int SpaceBullet::recover_from_penetration_ray(RigidBodyBullet *p_body, const btT btCompoundShape *cs = static_cast(otherObject->getCollisionShape()); for (int x = cs->getNumChildShapes() - 1; 0 <= x; --x) { - RecoverResult r_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, &r_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)) { - const btRigidBody *btRigid = static_cast(otherObject); - CollisionObjectBullet *collisionObject = static_cast(otherObject->getUserPointer()); - - r_results[ray_index].collision_depth = r_recover_result.penetration_distance; - B_TO_G(r_recover_result.pointWorld, r_results[ray_index].collision_point); - B_TO_G(r_recover_result.normal, r_results[ray_index].collision_normal); - B_TO_G(btRigid->getVelocityInLocalPoint(r_recover_result.pointWorld - btRigid->getWorldTransform().getOrigin()), r_results[ray_index].collider_velocity); - r_results[ray_index].collision_local_shape = kinIndex; - r_results[ray_index].collider_id = collisionObject->get_instance_id(); - r_results[ray_index].collider = collisionObject->get_self(); - r_results[ray_index].collider_shape = r_recover_result.other_compound_shape_index; + convert_to_separation_result(&r_results[ray_index], 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)) { + + convert_to_separation_result(&r_results[ray_index], recover_result, kinIndex, otherObject); + } } } diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h index 9f36c639822..7bf6a216b58 100644 --- a/modules/bullet/space_bullet.h +++ b/modules/bullet/space_bullet.h @@ -212,6 +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 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