Optimized in case is used just 1 shape with no transform

7 files changed, 78 insertions, 36 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/collision_object_bullet.cpp b/modules/bullet/collision_object_bullet.cpp
index 271cdb0223..c1e999f3f6 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),
@@ -186,13 +193,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 +285,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 +306,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 +357,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/rigid_body_bullet.cpp b/modules/bullet/rigid_body_bullet.cpp
index 9c0e802be5..3088ecb9dd 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,6 +314,10 @@ 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);
@@ -783,9 +787,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 +992,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/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index e8e6fc4d07..00ed1cbff3 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;
@@ -684,18 +683,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()) {