Merge pull request #23897 from AndreaCatania/fix_cd

Improved algorithm that check collision

modules/bullet/rigid_body_bullet.cpp

@@ -268,6 +268,7 @@ RigidBodyBullet::RigidBodyBullet() :
 		can_integrate_forces(false),
 		maxCollisionsDetection(0),
 		collisionsCount(0),
+		prev_collision_count(0),
 		maxAreasWhereIam(10),
 		areaWhereIamCount(0),
 		countGravityPointSpaces(0),
@@ -293,6 +294,9 @@ RigidBodyBullet::RigidBodyBullet() :
 		areasWhereIam.write[i] = NULL;
 	}
 	btBody->setSleepingThresholds(0.2, 0.2);
+
+	prev_collision_traces = &collision_traces_1;
+	curr_collision_traces = &collision_traces_2;
 }
 
 RigidBodyBullet::~RigidBodyBullet() {
@@ -410,7 +414,14 @@ void RigidBodyBullet::on_collision_filters_change() {
 }
 
 void RigidBodyBullet::on_collision_checker_start() {
+
+	prev_collision_count = collisionsCount;
 	collisionsCount = 0;
+
+	// Swap array
+	Vector<RigidBodyBullet *> *s = prev_collision_traces;
+	prev_collision_traces = curr_collision_traces;
+	curr_collision_traces = s;
 }
 
 void RigidBodyBullet::on_collision_checker_end() {
@@ -433,10 +444,20 @@ bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const
 	cd.other_object_shape = p_other_shape_index;
 	cd.local_shape = p_local_shape_index;
 
+	curr_collision_traces->write[collisionsCount] = p_otherObject;
+
 	++collisionsCount;
 	return true;
 }
 
+bool RigidBodyBullet::was_colliding(RigidBodyBullet *p_other_object) {
+	for (int i = prev_collision_count - 1; 0 <= i; --i) {
+		if ((*prev_collision_traces)[i] == p_other_object)
+			return true;
+	}
+	return false;
+}
+
 void RigidBodyBullet::assert_no_constraints() {
 	if (btBody->getNumConstraintRefs()) {
 		WARN_PRINT("A body with a joints is destroyed. Please check the implementation in order to destroy the joint before the body.");

modules/bullet/rigid_body_bullet.h

@@ -205,9 +205,15 @@ private:
 	bool can_integrate_forces;
 
 	Vector<CollisionData> collisions;
+	Vector<RigidBodyBullet *> collision_traces_1;
+	Vector<RigidBodyBullet *> collision_traces_2;
+	Vector<RigidBodyBullet *> *prev_collision_traces;
+	Vector<RigidBodyBullet *> *curr_collision_traces;
+
 	// these parameters are used to avoid vector resize
 	int maxCollisionsDetection;
 	int collisionsCount;
+	int prev_collision_count;
 
 	Vector<AreaBullet *> areasWhereIam;
 	// these parameters are used to avoid vector resize
@@ -244,9 +250,17 @@ public:
 	virtual void on_collision_checker_end();
 
 	void set_max_collisions_detection(int p_maxCollisionsDetection) {
+
+		ERR_FAIL_COND(0 > p_maxCollisionsDetection);
+
 		maxCollisionsDetection = p_maxCollisionsDetection;
+
 		collisions.resize(p_maxCollisionsDetection);
+		collision_traces_1.resize(p_maxCollisionsDetection);
+		collision_traces_2.resize(p_maxCollisionsDetection);
+
 		collisionsCount = 0;
+		prev_collision_count = MIN(prev_collision_count, p_maxCollisionsDetection);
 	}
 	int get_max_collisions_detection() {
 		return maxCollisionsDetection;
@@ -254,6 +268,7 @@ public:
 
 	bool can_add_collision() { return collisionsCount < maxCollisionsDetection; }
 	bool add_collision_object(RigidBodyBullet *p_otherObject, const Vector3 &p_hitWorldLocation, const Vector3 &p_hitLocalLocation, const Vector3 &p_hitNormal, const float &p_appliedImpulse, int p_other_shape_index, int p_local_shape_index);
+	bool was_colliding(RigidBodyBullet *p_other_object);
 
 	void assert_no_constraints();
 

modules/bullet/space_bullet.cpp

@@ -786,16 +786,22 @@ void SpaceBullet::check_body_collision() {
 			}
 
 			const int numContacts = contactManifold->getNumContacts();
+
+			/// Since I don't need report all contacts for these objects,
+			/// So report only the first
 #define REPORT_ALL_CONTACTS 0
 #if REPORT_ALL_CONTACTS
 			for (int j = 0; j < numContacts; j++) {
 				btManifoldPoint &pt = contactManifold->getContactPoint(j);
 #else
-			// Since I don't need report all contacts for these objects, I'll report only the first
 			if (numContacts) {
 				btManifoldPoint &pt = contactManifold->getContactPoint(0);
 #endif
-				if (pt.getDistance() <= 0.0) {
+				if (
+						pt.getDistance() <= 0.0 ||
+						bodyA->was_colliding(bodyB) ||
+						bodyB->was_colliding(bodyA)) {
+
 					Vector3 collisionWorldPosition;
 					Vector3 collisionLocalPosition;
 					Vector3 normalOnB;