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.

5 files changed, 32 insertions, 29 deletions

diff --git a/modules/bullet/btRayShape.cpp b/modules/bullet/btRayShape.cpp
index 935d86daa6..b902d08eca 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 449f625e17..cadc8dd59e 100644
--- a/modules/bullet/godot_ray_world_algorithm.cpp
+++ b/modules/bullet/godot_ray_world_algorithm.cpp
@@ -35,8 +35,6 @@
 
 #include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
 
-#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 7babfcc133..360950c4c7 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 6562b18b3c..40db5f8a23 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<const btRigidBody *>(p_other_object);
+	CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(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<btCompoundShape *>(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<const btRigidBody *>(otherObject);
-						CollisionObjectBullet *collisionObject = static_cast<CollisionObjectBullet *>(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 9f36c63982..7bf6a216b5 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