Merge pull request #21808 from AndreaCatania/optitri

Optimized bullet trimesh collision

12 files changed, 101 insertions, 40 deletions

diff --git a/modules/bullet/area_bullet.cpp b/modules/bullet/area_bullet.cpp
index 3668088590..f1da454e2e 100644
--- a/modules/bullet/area_bullet.cpp
+++ b/modules/bullet/area_bullet.cpp
@@ -57,7 +57,6 @@ AreaBullet::AreaBullet() :
 		spOv_priority(0) {
 
 	btGhost = bulletnew(btGhostObject);
-	btGhost->setCollisionShape(compoundShape);
 	setupBulletCollisionObject(btGhost);
 	/// Collision objects with a callback still have collision response with dynamic rigid bodies.
 	/// In order to use collision objects as trigger, you have to disable the collision response.
@@ -162,6 +161,10 @@ bool AreaBullet::is_monitoring() const {
 	return get_godot_object_flags() & GOF_IS_MONITORING_AREA;
 }
 
+void AreaBullet::main_shape_resetted() {
+	btGhost->setCollisionShape(get_main_shape());
+}
+
 void AreaBullet::reload_body() {
 	if (space) {
 		space->remove_area(this);
diff --git a/modules/bullet/area_bullet.h b/modules/bullet/area_bullet.h
index b2046c684e..a6bf73906c 100644
--- a/modules/bullet/area_bullet.h
+++ b/modules/bullet/area_bullet.h
@@ -142,6 +142,7 @@ public:
 	_FORCE_INLINE_ void set_spOv_priority(int p_priority) { spOv_priority = p_priority; }
 	_FORCE_INLINE_ int get_spOv_priority() { return spOv_priority; }
 
+	virtual void main_shape_resetted();
 	virtual void reload_body();
 	virtual void set_space(SpaceBullet *p_space);
 
diff --git a/modules/bullet/bullet_utilities.h b/modules/bullet/bullet_utilities.h
index 2841dfbe69..029eb6691a 100644
--- a/modules/bullet/bullet_utilities.h
+++ b/modules/bullet/bullet_utilities.h
@@ -35,8 +35,6 @@
 	@author AndreaCatania
 */
 
-#pragma once
-
 #define bulletnew(cl) \
 	new cl
 
diff --git a/modules/bullet/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index 271cdb0223..df67f8d7ab 100644
--- a/modules/bullet/collision_object_bullet.cpp
+++ b/modules/bullet/collision_object_bullet.cpp
@@ -53,10 +53,17 @@ void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform &p_trans
 	G_TO_B(p_transform, transform);
 	UNSCALE_BT_BASIS(transform);
 }
+
 void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_transform) {
 	transform = p_transform;
 }
 
+void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_scale) {
+	if (!bt_shape) {
+		bt_shape = shape->create_bt_shape(scale * body_scale);
+	}
+}
+
 CollisionObjectBullet::CollisionObjectBullet(Type p_type) :
 		RIDBullet(),
 		space(NULL),
@@ -107,6 +114,7 @@ void CollisionObjectBullet::setupBulletCollisionObject(btCollisionObject *p_coll
 	bt_collision_object->setUserIndex(type);
 	// Force the enabling of collision and avoid problems
 	set_collision_enabled(collisionsEnabled);
+	p_collisionObject->setCollisionFlags(p_collisionObject->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
 }
 
 void CollisionObjectBullet::add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject) {
@@ -186,13 +194,14 @@ const btTransform &CollisionObjectBullet::get_transform__bullet() const {
 
 RigidCollisionObjectBullet::RigidCollisionObjectBullet(Type p_type) :
 		CollisionObjectBullet(p_type),
-		compoundShape(bulletnew(btCompoundShape(enableDynamicAabbTree, initialChildCapacity))) {
+		mainShape(NULL) {
 }
 
 RigidCollisionObjectBullet::~RigidCollisionObjectBullet() {
 	remove_all_shapes(true);
-	bt_collision_object->setCollisionShape(NULL);
-	bulletdelete(compoundShape);
+	if (mainShape && mainShape->isCompound()) {
+		bulletdelete(mainShape);
+	}
 }
 
 /* Not used
@@ -277,6 +286,10 @@ btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
 	return shapes[p_index].bt_shape;
 }
 
+const btTransform &RigidCollisionObjectBullet::get_bt_shape_transform(int p_index) const {
+	return shapes[p_index].transform;
+}
+
 Transform RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
 	Transform trs;
 	B_TO_G(shapes[p_index].transform, trs);
@@ -294,33 +307,47 @@ void RigidCollisionObjectBullet::on_shape_changed(const ShapeBullet *const p_sha
 }
 
 void RigidCollisionObjectBullet::on_shapes_changed() {
-	int i;
 
-	// Remove all shapes, reverse order for performance reason (Array resize)
-	for (i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
-		compoundShape->removeChildShapeByIndex(i);
+	if (mainShape && mainShape->isCompound()) {
+		bulletdelete(mainShape);
 	}
+	mainShape = NULL;
 
 	ShapeWrapper *shpWrapper;
-	const int shapes_size = shapes.size();
+	const int shape_count = shapes.size();
 
 	// Reset shape if required
 	if (force_shape_reset) {
-		for (i = 0; i < shapes_size; ++i) {
+		for (int i(0); i < shape_count; ++i) {
 			shpWrapper = &shapes.write[i];
 			bulletdelete(shpWrapper->bt_shape);
 		}
 		force_shape_reset = false;
 	}
 
-	// Insert all shapes
 	btVector3 body_scale(get_bt_body_scale());
-	for (i = 0; i < shapes_size; ++i) {
+
+	if (!shape_count)
+		return;
+
+	// Try to optimize by not using compound
+	if (1 == shape_count) {
+		shpWrapper = &shapes.write[0];
+		if (shpWrapper->active && shpWrapper->transform.getOrigin().isZero() && shpWrapper->transform.getBasis() == shpWrapper->transform.getBasis().getIdentity()) {
+			shpWrapper->claim_bt_shape(body_scale);
+			mainShape = shpWrapper->bt_shape;
+			main_shape_resetted();
+			return;
+		}
+	}
+
+	btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, initialChildCapacity));
+
+	// Insert all shapes into compound
+	for (int i(0); i < shape_count; ++i) {
 		shpWrapper = &shapes.write[i];
 		if (shpWrapper->active) {
-			if (!shpWrapper->bt_shape) {
-				shpWrapper->bt_shape = shpWrapper->shape->create_bt_shape(shpWrapper->scale * body_scale);
-			}
+			shpWrapper->claim_bt_shape(body_scale);
 
 			btTransform scaled_shape_transform(shpWrapper->transform);
 			scaled_shape_transform.getOrigin() *= body_scale;
@@ -331,6 +358,8 @@ void RigidCollisionObjectBullet::on_shapes_changed() {
 	}
 
 	compoundShape->recalculateLocalAabb();
+	mainShape = compoundShape;
+	main_shape_resetted();
 }
 
 void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
diff --git a/modules/bullet/collision_object_bullet.h b/modules/bullet/collision_object_bullet.h
index 506976eabf..d14fdd3301 100644
--- a/modules/bullet/collision_object_bullet.h
+++ b/modules/bullet/collision_object_bullet.h
@@ -109,6 +109,8 @@ public:
 
 		void set_transform(const Transform &p_transform);
 		void set_transform(const btTransform &p_transform);
+
+		void claim_bt_shape(const btVector3 &body_scale);
 	};
 
 protected:
@@ -207,10 +209,8 @@ public:
 
 class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet {
 protected:
-	/// This is required to combine some shapes together.
-	/// Since Godot allow to have multiple shapes for each body with custom relative location,
-	/// each body will attach the shapes using this class even if there is only one shape.
-	btCompoundShape *compoundShape;
+	/// This could be a compound shape in case multi please collision are found
+	btCollisionShape *mainShape;
 	Vector<ShapeWrapper> shapes;
 
 public:
@@ -231,15 +231,18 @@ public:
 	virtual void on_shape_changed(const ShapeBullet *const p_shape);
 	virtual void on_shapes_changed();
 
-	_FORCE_INLINE_ btCompoundShape *get_compound_shape() const { return compoundShape; }
+	_FORCE_INLINE_ btCollisionShape *get_main_shape() const { return mainShape; }
+
 	int get_shape_count() const;
 	ShapeBullet *get_shape(int p_index) const;
 	btCollisionShape *get_bt_shape(int p_index) const;
+	const btTransform &get_bt_shape_transform(int p_index) const;
 	Transform get_shape_transform(int p_index) const;
 
 	void set_shape_disabled(int p_index, bool p_disabled);
 	bool is_shape_disabled(int p_index);
 
+	virtual void main_shape_resetted() = 0;
 	virtual void on_body_scale_changed();
 
 private:
diff --git a/modules/bullet/godot_result_callbacks.cpp b/modules/bullet/godot_result_callbacks.cpp
index 534034d707..08d8b8c6f6 100644
--- a/modules/bullet/godot_result_callbacks.cpp
+++ b/modules/bullet/godot_result_callbacks.cpp
@@ -34,11 +34,19 @@
 #include "bullet_types_converter.h"
 #include "collision_object_bullet.h"
 #include "rigid_body_bullet.h"
+#include <BulletCollision/CollisionDispatch/btInternalEdgeUtility.h>
 
 /**
 	@author AndreaCatania
 */
 
+bool godotContactAddedCallback(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
+	if (!colObj1Wrap->getCollisionObject()->getCollisionShape()->isCompound()) {
+		btAdjustInternalEdgeContacts(cp, colObj1Wrap, colObj0Wrap, partId1, index1);
+	}
+	return true;
+}
+
 bool GodotFilterCallback::test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask) {
 	return body0_collision_layer & body1_collision_mask || body1_collision_layer & body0_collision_mask;
 }
diff --git a/modules/bullet/godot_result_callbacks.h b/modules/bullet/godot_result_callbacks.h
index 3948f43c00..61247e2a7a 100644
--- a/modules/bullet/godot_result_callbacks.h
+++ b/modules/bullet/godot_result_callbacks.h
@@ -42,6 +42,9 @@
 
 class RigidBodyBullet;
 
+/// This callback is injected inside bullet server and allow me to smooth contacts against trimesh
+bool godotContactAddedCallback(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1);
+
 /// This class is required to implement custom collision behaviour in the broadphase
 struct GodotFilterCallback : public btOverlapFilterCallback {
 	static bool test_collision_filters(uint32_t body0_collision_layer, uint32_t body0_collision_mask, uint32_t body1_collision_layer, uint32_t body1_collision_mask);
diff --git a/modules/bullet/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index 9c0e802be5..2974d6b3e4 100644
--- a/modules/bullet/rigid_body_bullet.cpp
+++ b/modules/bullet/rigid_body_bullet.cpp
@@ -279,7 +279,7 @@ RigidBodyBullet::RigidBodyBullet() :
 
 	// Initial properties
 	const btVector3 localInertia(0, 0, 0);
-	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, compoundShape, localInertia);
+	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, NULL, localInertia);
 
 	btBody = bulletnew(btRigidBody(cInfo));
 	setupBulletCollisionObject(btBody);
@@ -314,10 +314,15 @@ void RigidBodyBullet::destroy_kinematic_utilities() {
 	}
 }
 
+void RigidBodyBullet::main_shape_resetted() {
+	btBody->setCollisionShape(get_main_shape());
+}
+
 void RigidBodyBullet::reload_body() {
 	if (space) {
 		space->remove_rigid_body(this);
-		space->add_rigid_body(this);
+		if (get_main_shape())
+			space->add_rigid_body(this);
 	}
 }
 
@@ -783,9 +788,11 @@ void RigidBodyBullet::on_shapes_changed() {
 	const btScalar invMass = btBody->getInvMass();
 	const btScalar mass = invMass == 0 ? 0 : 1 / invMass;
 
-	btVector3 inertia;
-	btBody->getCollisionShape()->calculateLocalInertia(mass, inertia);
-	btBody->setMassProps(mass, inertia);
+	if (mainShape) {
+		btVector3 inertia;
+		mainShape->calculateLocalInertia(mass, inertia);
+		btBody->setMassProps(mass, inertia);
+	}
 	btBody->updateInertiaTensor();
 
 	reload_kinematic_shapes();
@@ -986,7 +993,8 @@ void RigidBodyBullet::_internal_set_mass(real_t p_mass) {
 			return;
 
 		m_isStatic = false;
-		compoundShape->calculateLocalInertia(p_mass, localInertia);
+		if (mainShape)
+			mainShape->calculateLocalInertia(p_mass, localInertia);
 
 		if (PhysicsServer::BODY_MODE_RIGID == mode) {
 
diff --git a/modules/bullet/rigid_body_bullet.h b/modules/bullet/rigid_body_bullet.h
index f03009bce9..cd2f215906 100644
--- a/modules/bullet/rigid_body_bullet.h
+++ b/modules/bullet/rigid_body_bullet.h
@@ -231,6 +231,7 @@ public:
 
 	_FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; }
 
+	virtual void main_shape_resetted();
 	virtual void reload_body();
 	virtual void set_space(SpaceBullet *p_space);
 
diff --git a/modules/bullet/shape_bullet.cpp b/modules/bullet/shape_bullet.cpp
index cc7d106496..4a2263407c 100644
--- a/modules/bullet/shape_bullet.cpp
+++ b/modules/bullet/shape_bullet.cpp
@@ -36,6 +36,7 @@
 #include "bullet_utilities.h"
 #include "shape_owner_bullet.h"
 
+#include <BulletCollision/CollisionDispatch/btInternalEdgeUtility.h>
 #include <BulletCollision/CollisionShapes/btConvexPointCloudShape.h>
 #include <BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h>
 #include <btBulletCollisionCommon.h>
@@ -358,7 +359,8 @@ ConcavePolygonShapeBullet::ConcavePolygonShapeBullet() :
 ConcavePolygonShapeBullet::~ConcavePolygonShapeBullet() {
 	if (meshShape) {
 		delete meshShape->getMeshInterface();
-		delete meshShape;
+		delete meshShape->getTriangleInfoMap();
+		bulletdelete(meshShape);
 	}
 	faces = PoolVector<Vector3>();
 }
@@ -380,6 +382,7 @@ void ConcavePolygonShapeBullet::setup(PoolVector<Vector3> p_faces) {
 	if (meshShape) {
 		/// Clear previous created shape
 		delete meshShape->getMeshInterface();
+		delete meshShape->getTriangleInfoMap();
 		bulletdelete(meshShape);
 	}
 	int src_face_count = faces.size();
@@ -407,6 +410,8 @@ void ConcavePolygonShapeBullet::setup(PoolVector<Vector3> p_faces) {
 		const bool useQuantizedAabbCompression = true;
 
 		meshShape = bulletnew(btBvhTriangleMeshShape(shapeInterface, useQuantizedAabbCompression));
+		btTriangleInfoMap *triangleInfoMap = new btTriangleInfoMap();
+		btGenerateInternalEdgeInfo(meshShape, triangleInfoMap);
 	} else {
 		meshShape = NULL;
 		ERR_PRINT("The faces count are 0, the mesh shape cannot be created");
diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index e8e6fc4d07..6d00b3d1d8 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -295,11 +295,10 @@ Vector3 BulletPhysicsDirectSpaceState::get_closest_point_to_object_volume(RID p_
 
 	bool shapes_found = false;
 
-	btCompoundShape *compound = rigid_object->get_compound_shape();
-	for (int i = compound->getNumChildShapes() - 1; 0 <= i; --i) {
-		shape = compound->getChildShape(i);
+	for (int i = rigid_object->get_shape_count() - 1; 0 <= i; --i) {
+		shape = rigid_object->get_bt_shape(i);
 		if (shape->isConvex()) {
-			child_transform = compound->getChildTransform(i);
+			child_transform = rigid_object->get_bt_shape_transform(i);
 			convex_shape = static_cast<btConvexShape *>(shape);
 
 			input.m_transformB = body_transform * child_transform;
@@ -598,6 +597,7 @@ void SpaceBullet::create_empty_world(bool p_create_soft_world) {
 	godotFilterCallback = bulletnew(GodotFilterCallback);
 	gCalculateCombinedRestitutionCallback = &calculateGodotCombinedRestitution;
 	gCalculateCombinedFrictionCallback = &calculateGodotCombinedFriction;
+	gContactAddedCallback = &godotContactAddedCallback;
 
 	dynamicsWorld->setWorldUserInfo(this);
 
@@ -684,18 +684,18 @@ void SpaceBullet::check_ghost_overlaps() {
 			bool hasOverlap = false;
 
 			// For each area shape
-			for (y = area->get_compound_shape()->getNumChildShapes() - 1; 0 <= y; --y) {
-				if (!area->get_compound_shape()->getChildShape(y)->isConvex())
+			for (y = area->get_shape_count() - 1; 0 <= y; --y) {
+				if (!area->get_bt_shape(y)->isConvex())
 					continue;
 
-				gjk_input.m_transformA = area->get_transform__bullet() * area->get_compound_shape()->getChildTransform(y);
-				area_shape = static_cast<btConvexShape *>(area->get_compound_shape()->getChildShape(y));
+				gjk_input.m_transformA = area->get_transform__bullet() * area->get_bt_shape_transform(y);
+				area_shape = static_cast<btConvexShape *>(area->get_bt_shape(y));
 
 				// For each other object shape
-				for (z = otherObject->get_compound_shape()->getNumChildShapes() - 1; 0 <= z; --z) {
+				for (z = otherObject->get_shape_count() - 1; 0 <= z; --z) {
 
-					other_body_shape = static_cast<btCollisionShape *>(otherObject->get_compound_shape()->getChildShape(z));
-					gjk_input.m_transformB = otherObject->get_transform__bullet() * otherObject->get_compound_shape()->getChildTransform(z);
+					other_body_shape = static_cast<btCollisionShape *>(otherObject->get_bt_shape(z));
+					gjk_input.m_transformB = otherObject->get_transform__bullet() * otherObject->get_bt_shape_transform(z);
 
 					if (other_body_shape->isConvex()) {
 
diff --git a/modules/bullet/space_bullet.h b/modules/bullet/space_bullet.h
index 517ec67ffa..0649e1f7e3 100644
--- a/modules/bullet/space_bullet.h
+++ b/modules/bullet/space_bullet.h
@@ -65,6 +65,8 @@ class SpaceBullet;
 class SoftBodyBullet;
 class btGjkEpaPenetrationDepthSolver;
 
+extern ContactAddedCallback gContactAddedCallback;
+
 class BulletPhysicsDirectSpaceState : public PhysicsDirectSpaceState {
 	GDCLASS(BulletPhysicsDirectSpaceState, PhysicsDirectSpaceState)
 private: