Merge pull request #25788 from aqnuep/rayshape_fix

Fix RayShape collision when used with a KinematicBody (Bullet Physics)

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));
 }

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);

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;
 	}
 }

modules/bullet/space_bullet.cpp

@@ -1246,6 +1246,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());
@@ -1297,22 +1312,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);
+				}
 			}
 		}
 

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