Merge pull request #17959 from AndreaCatania/kinfix2

Fixed kinematic sliding on trimesh

modules/bullet/godot_result_callbacks.cpp

@@ -257,15 +257,12 @@ btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp
 
 void GodotDeepPenetrationContactResultCallback::addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth) {
 
-	// Has penetration
-	if (m_penetration_distance < ABS(depth)) {
+	if (m_penetration_distance > depth) { // Has penetration?
 
 		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_other_compound_shape_index = isSwapped ? m_index0 : m_index1;
 		m_pointNormalWorld = isSwapped ? normalOnBInWorld * -1 : normalOnBInWorld;
-		m_pointWorld = isSwapped ? (pointInWorldOnB + normalOnBInWorld * depth) : pointInWorldOnB;
+		m_pointWorld = isSwapped ? (pointInWorldOnB + (normalOnBInWorld * depth)) : pointInWorldOnB;
 	}
 }

modules/bullet/godot_result_callbacks.h

@@ -185,21 +185,18 @@ struct GodotDeepPenetrationContactResultCallback : public btManifoldResult {
 	btVector3 m_pointWorld;
 	btScalar m_penetration_distance;
 	int m_other_compound_shape_index;
-	const btCollisionObject *m_pointCollisionObject;
 
 	GodotDeepPenetrationContactResultCallback(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap) :
 			btManifoldResult(body0Wrap, body1Wrap),
-			m_pointCollisionObject(NULL),
 			m_penetration_distance(0),
 			m_other_compound_shape_index(0) {}
 
 	void reset() {
-		m_pointCollisionObject = NULL;
 		m_penetration_distance = 0;
 	}
 
 	bool hasHit() {
-		return m_pointCollisionObject;
+		return m_penetration_distance < 0;
 	}
 
 	virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorldOnB, btScalar depth);

modules/bullet/space_bullet.cpp

@@ -1120,7 +1120,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 * p_recover_movement_scale);
+			r_delta_recover_movement += contactPointResult.m_pointNormalWorld * (contactPointResult.m_penetration_distance * -1 * p_recover_movement_scale);
 
 			if (r_recover_result) {
 				if (contactPointResult.m_penetration_distance < r_recover_result->penetration_distance) {