Improved code that handles collision shapes, fixes #21945

modules/bullet/area_bullet.cpp

@@ -58,7 +58,7 @@ AreaBullet::AreaBullet() :
 		isScratched(false) {
 
 	btGhost = bulletnew(btGhostObject);
-	btGhost->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+	reload_shapes();
 	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.
@@ -166,11 +166,9 @@ bool AreaBullet::is_monitoring() const {
 	return get_godot_object_flags() & GOF_IS_MONITORING_AREA;
 }
 
-void AreaBullet::main_shape_resetted() {
-	if (get_main_shape())
-		btGhost->setCollisionShape(get_main_shape());
-	else
-		btGhost->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+void AreaBullet::main_shape_changed() {
+	CRASH_COND(!get_main_shape())
+	btGhost->setCollisionShape(get_main_shape());
 }
 
 void AreaBullet::reload_body() {

modules/bullet/area_bullet.h

@@ -142,7 +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 main_shape_changed();
 	virtual void reload_body();
 	virtual void set_space(SpaceBullet *p_space);
 

modules/bullet/bullet_physics_server.cpp

@@ -74,12 +74,6 @@
 	body->get_space()->add_constraint(joint, joint->is_disabled_collisions_between_bodies());
 // <--------------- Joint creation asserts
 
-btEmptyShape *BulletPhysicsServer::emptyShape(ShapeBullet::create_shape_empty());
-
-btEmptyShape *BulletPhysicsServer::get_empty_shape() {
-	return emptyShape;
-}
-
 void BulletPhysicsServer::_bind_methods() {
 	//ClassDB::bind_method(D_METHOD("DoTest"), &BulletPhysicsServer::DoTest);
 }
@@ -89,9 +83,7 @@ BulletPhysicsServer::BulletPhysicsServer() :
 		active(true),
 		active_spaces_count(0) {}
 
-BulletPhysicsServer::~BulletPhysicsServer() {
-	bulletdelete(emptyShape);
-}
+BulletPhysicsServer::~BulletPhysicsServer() {}
 
 RID BulletPhysicsServer::shape_create(ShapeType p_shape) {
 	ShapeBullet *shape = NULL;
@@ -338,7 +330,7 @@ Transform BulletPhysicsServer::area_get_shape_transform(RID p_area, int p_shape_
 void BulletPhysicsServer::area_remove_shape(RID p_area, int p_shape_idx) {
 	AreaBullet *area = area_owner.get(p_area);
 	ERR_FAIL_COND(!area);
-	return area->remove_shape(p_shape_idx);
+	return area->remove_shape_full(p_shape_idx);
 }
 
 void BulletPhysicsServer::area_clear_shapes(RID p_area) {
@@ -346,7 +338,7 @@ void BulletPhysicsServer::area_clear_shapes(RID p_area) {
 	ERR_FAIL_COND(!area);
 
 	for (int i = area->get_shape_count(); 0 < i; --i)
-		area->remove_shape(0);
+		area->remove_shape_full(0);
 }
 
 void BulletPhysicsServer::area_set_shape_disabled(RID p_area, int p_shape_idx, bool p_disabled) {
@@ -567,7 +559,7 @@ void BulletPhysicsServer::body_remove_shape(RID p_body, int p_shape_idx) {
 	RigidBodyBullet *body = rigid_body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->remove_shape(p_shape_idx);
+	body->remove_shape_full(p_shape_idx);
 }
 
 void BulletPhysicsServer::body_clear_shapes(RID p_body) {
@@ -1486,7 +1478,7 @@ void BulletPhysicsServer::free(RID p_rid) {
 
 		// Notify the shape is configured
 		for (Map<ShapeOwnerBullet *, int>::Element *element = shape->get_owners().front(); element; element = element->next()) {
-			static_cast<ShapeOwnerBullet *>(element->key())->remove_shape(shape);
+			static_cast<ShapeOwnerBullet *>(element->key())->remove_shape_full(shape);
 		}
 
 		shape_owner.free(p_rid);

modules/bullet/bullet_physics_server.h

@@ -60,13 +60,6 @@ class BulletPhysicsServer : public PhysicsServer {
 	mutable RID_Owner<SoftBodyBullet> soft_body_owner;
 	mutable RID_Owner<JointBullet> joint_owner;
 
-private:
-	/// This is used as replacement of collision shape inside a compound or main shape
-	static btEmptyShape *emptyShape;
-
-public:
-	static btEmptyShape *get_empty_shape();
-
 protected:
 	static void _bind_methods();
 

modules/bullet/collision_object_bullet.cpp

@@ -43,8 +43,7 @@
 	@author AndreaCatania
 */
 
-#define enableDynamicAabbTree true
-#define initialChildCapacity 1
+#define enableDynamicAabbTree false
 
 CollisionObjectBullet::ShapeWrapper::~ShapeWrapper() {}
 
@@ -60,7 +59,10 @@ void CollisionObjectBullet::ShapeWrapper::set_transform(const btTransform &p_tra
 
 void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_scale) {
 	if (!bt_shape) {
-		bt_shape = shape->create_bt_shape(scale * body_scale);
+		if (active)
+			bt_shape = shape->create_bt_shape(scale * body_scale);
+		else
+			bt_shape = ShapeBullet::create_shape_empty();
 	}
 }
 
@@ -90,7 +92,7 @@ bool equal(real_t first, real_t second) {
 void CollisionObjectBullet::set_body_scale(const Vector3 &p_new_scale) {
 	if (!equal(p_new_scale[0], body_scale[0]) || !equal(p_new_scale[1], body_scale[1]) || !equal(p_new_scale[2], body_scale[2])) {
 		body_scale = p_new_scale;
-		on_body_scale_changed();
+		body_scale_changed();
 	}
 }
 
@@ -100,7 +102,7 @@ btVector3 CollisionObjectBullet::get_bt_body_scale() const {
 	return s;
 }
 
-void CollisionObjectBullet::on_body_scale_changed() {
+void CollisionObjectBullet::body_scale_changed() {
 	force_shape_reset = true;
 }
 
@@ -204,31 +206,10 @@ RigidCollisionObjectBullet::~RigidCollisionObjectBullet() {
 	}
 }
 
-/* Not used
-void RigidCollisionObjectBullet::_internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) {
-	bool at_least_one_was_changed = false;
-	btTransform old_transf;
-	// Inverse because I need remove the shapes
-	// Fetch all shapes to be sure to remove all shapes
-	for (int i = compoundShape->getNumChildShapes() - 1; 0 <= i; --i) {
-		if (compoundShape->getChildShape(i) == p_old_shape) {
-
-			old_transf = compoundShape->getChildTransform(i);
-			compoundShape->removeChildShapeByIndex(i);
-			compoundShape->addChildShape(old_transf, p_new_shape);
-			at_least_one_was_changed = true;
-		}
-	}
-
-	if (at_least_one_was_changed) {
-		on_shapes_changed();
-	}
-}*/
-
 void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform &p_transform) {
 	shapes.push_back(ShapeWrapper(p_shape, p_transform, true));
 	p_shape->add_owner(this);
-	on_shapes_changed();
+	reload_shapes();
 }
 
 void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
@@ -236,17 +217,31 @@ void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
 	shp.shape->remove_owner(this);
 	p_shape->add_owner(this);
 	shp.shape = p_shape;
-	on_shapes_changed();
+	reload_shapes();
 }
 
-void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) {
-	ERR_FAIL_INDEX(p_index, get_shape_count());
+int RigidCollisionObjectBullet::get_shape_count() const {
+	return shapes.size();
+}
 
-	shapes.write[p_index].set_transform(p_transform);
-	on_shape_changed(shapes.write[p_index].shape);
+ShapeBullet *RigidCollisionObjectBullet::get_shape(int p_index) const {
+	return shapes[p_index].shape;
 }
 
-void RigidCollisionObjectBullet::remove_shape(ShapeBullet *p_shape) {
+btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
+	return shapes[p_index].bt_shape;
+}
+
+int RigidCollisionObjectBullet::find_shape(ShapeBullet *p_shape) const {
+	const int size = shapes.size();
+	for (int i = 0; i < size; ++i) {
+		if (shapes[i].shape == p_shape)
+			return i;
+	}
+	return -1;
+}
+
+void RigidCollisionObjectBullet::remove_shape_full(ShapeBullet *p_shape) {
 	// Remove the shape, all the times it appears
 	// Reverse order required for delete.
 	for (int i = shapes.size() - 1; 0 <= i; --i) {
@@ -255,14 +250,14 @@ void RigidCollisionObjectBullet::remove_shape(ShapeBullet *p_shape) {
 			shapes.remove(i);
 		}
 	}
-	on_shapes_changed();
+	reload_shapes();
 }
 
-void RigidCollisionObjectBullet::remove_shape(int p_index) {
+void RigidCollisionObjectBullet::remove_shape_full(int p_index) {
 	ERR_FAIL_INDEX(p_index, get_shape_count());
 	internal_shape_destroy(p_index);
 	shapes.remove(p_index);
-	on_shapes_changed();
+	reload_shapes();
 }
 
 void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBody) {
@@ -271,19 +266,15 @@ void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBod
 		internal_shape_destroy(i, p_permanentlyFromThisBody);
 	}
 	shapes.clear();
-	on_shapes_changed();
+	reload_shapes();
 }
 
-int RigidCollisionObjectBullet::get_shape_count() const {
-	return shapes.size();
-}
-
-ShapeBullet *RigidCollisionObjectBullet::get_shape(int p_index) const {
-	return shapes[p_index].shape;
-}
+void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) {
+	ERR_FAIL_INDEX(p_index, get_shape_count());
 
-btCollisionShape *RigidCollisionObjectBullet::get_bt_shape(int p_index) const {
-	return shapes[p_index].bt_shape;
+	shapes.write[p_index].set_transform(p_transform);
+	// Note, enableDynamicAabbTree is false because on transform change compound is destroyed
+	reload_shapes();
 }
 
 const btTransform &RigidCollisionObjectBullet::get_bt_shape_transform(int p_index) const {
@@ -296,21 +287,26 @@ Transform RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
 	return trs;
 }
 
-void RigidCollisionObjectBullet::on_shape_changed(const ShapeBullet *const p_shape) {
-	const int size = shapes.size();
-	for (int i = 0; i < size; ++i) {
-		if (shapes[i].shape == p_shape) {
-			bulletdelete(shapes.write[i].bt_shape);
-		}
-	}
-	on_shapes_changed();
+void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
+	shapes.write[p_index].active = !p_disabled;
+	shape_changed(p_index);
 }
 
-void RigidCollisionObjectBullet::on_shapes_changed() {
+bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
+	return !shapes[p_index].active;
+}
 
-	if (mainShape && mainShape->isCompound()) {
+void RigidCollisionObjectBullet::shape_changed(int p_shape_index) {
+	bulletdelete(shapes.write[p_shape_index].bt_shape);
+	reload_shapes();
+}
+
+void RigidCollisionObjectBullet::reload_shapes() {
+
+	if (mainShape && mainShape->isCompound())
+		// Destroy compound
 		bulletdelete(mainShape);
-	}
+
 	mainShape = NULL;
 
 	ShapeWrapper *shpWrapper;
@@ -325,7 +321,7 @@ void RigidCollisionObjectBullet::on_shapes_changed() {
 		force_shape_reset = false;
 	}
 
-	btVector3 body_scale(get_bt_body_scale());
+	const btVector3 body_scale(get_bt_body_scale());
 
 	if (!shape_count)
 		return;
@@ -333,47 +329,33 @@ void RigidCollisionObjectBullet::on_shapes_changed() {
 	// 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()) {
+		if (shpWrapper->transform.getOrigin().isZero() && shpWrapper->transform.getBasis() == shpWrapper->transform.getBasis().getIdentity()) {
 			shpWrapper->claim_bt_shape(body_scale);
 			mainShape = shpWrapper->bt_shape;
-			main_shape_resetted();
+			main_shape_changed();
 			return;
 		}
 	}
 
-	btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, initialChildCapacity));
+	// Optimization not possible use a compound shape
+	btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, shape_count));
 
-	// Insert all shapes into compound
 	for (int i(0); i < shape_count; ++i) {
 		shpWrapper = &shapes.write[i];
-		if (shpWrapper->active) {
-			shpWrapper->claim_bt_shape(body_scale);
-
-			btTransform scaled_shape_transform(shpWrapper->transform);
-			scaled_shape_transform.getOrigin() *= body_scale;
-			compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
-		} else {
-			compoundShape->addChildShape(btTransform(), BulletPhysicsServer::get_empty_shape());
-		}
+		shpWrapper->claim_bt_shape(body_scale);
+		btTransform scaled_shape_transform(shpWrapper->transform);
+		scaled_shape_transform.getOrigin() *= body_scale;
+		compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
 	}
 
 	compoundShape->recalculateLocalAabb();
 	mainShape = compoundShape;
-	main_shape_resetted();
-}
-
-void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled) {
-	shapes.write[p_index].active = !p_disabled;
-	on_shapes_changed();
-}
-
-bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
-	return !shapes[p_index].active;
+	main_shape_changed();
 }
 
-void RigidCollisionObjectBullet::on_body_scale_changed() {
-	CollisionObjectBullet::on_body_scale_changed();
-	on_shapes_changed();
+void RigidCollisionObjectBullet::body_scale_changed() {
+	CollisionObjectBullet::body_scale_changed();
+	reload_shapes();
 }
 
 void RigidCollisionObjectBullet::internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody) {

modules/bullet/collision_object_bullet.h

@@ -157,7 +157,7 @@ public:
 	void set_body_scale(const Vector3 &p_new_scale);
 	const Vector3 &get_body_scale() const { return body_scale; }
 	btVector3 get_bt_body_scale() const;
-	virtual void on_body_scale_changed();
+	virtual void body_scale_changed();
 
 	void add_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
 	void remove_collision_exception(const CollisionObjectBullet *p_ignoreCollisionObject);
@@ -185,7 +185,7 @@ public:
 	virtual void reload_body() = 0;
 	virtual void set_space(SpaceBullet *p_space) = 0;
 	_FORCE_INLINE_ SpaceBullet *get_space() const { return space; }
-	/// This is an event that is called when a collision checker starts
+
 	virtual void on_collision_checker_start() = 0;
 
 	virtual void dispatch_callbacks() = 0;
@@ -197,7 +197,6 @@ public:
 	virtual void on_enter_area(AreaBullet *p_area) = 0;
 	virtual void on_exit_area(AreaBullet *p_area);
 
-	/// GodotObjectFlags
 	void set_godot_object_flags(int flags);
 	int get_godot_object_flags() const;
 
@@ -209,7 +208,6 @@ public:
 
 class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet {
 protected:
-	/// This could be a compound shape in case multi please collision are found
 	btCollisionShape *mainShape;
 	Vector<ShapeWrapper> shapes;
 
@@ -219,31 +217,34 @@ public:
 
 	_FORCE_INLINE_ const Vector<ShapeWrapper> &get_shapes_wrappers() const { return shapes; }
 
-	/// This is used to set new shape or replace existing
-	//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
+	_FORCE_INLINE_ btCollisionShape *get_main_shape() const { return mainShape; }
+
 	void add_shape(ShapeBullet *p_shape, const Transform &p_transform = Transform());
 	void set_shape(int p_index, ShapeBullet *p_shape);
-	void set_shape_transform(int p_index, const Transform &p_transform);
-	virtual void remove_shape(ShapeBullet *p_shape);
-	void remove_shape(int p_index);
-	void remove_all_shapes(bool p_permanentlyFromThisBody = false);
-
-	virtual void on_shape_changed(const ShapeBullet *const p_shape);
-	virtual void on_shapes_changed();
-
-	_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;
+
+	int find_shape(ShapeBullet *p_shape) const;
+
+	virtual void remove_shape_full(ShapeBullet *p_shape);
+	void remove_shape_full(int p_index);
+	void remove_all_shapes(bool p_permanentlyFromThisBody = false);
+
+	void set_shape_transform(int p_index, const Transform &p_transform);
+
 	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();
+	virtual void shape_changed(int p_shape_index);
+	virtual void reload_shapes();
+
+	virtual void main_shape_changed() = 0;
+	virtual void body_scale_changed();
 
 private:
 	void internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody = false);

modules/bullet/rigid_body_bullet.cpp

@@ -279,9 +279,10 @@ RigidBodyBullet::RigidBodyBullet() :
 
 	// Initial properties
 	const btVector3 localInertia(0, 0, 0);
-	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, BulletPhysicsServer::get_empty_shape(), localInertia);
+	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, godotMotionState, NULL, localInertia);
 
 	btBody = bulletnew(btRigidBody(cInfo));
+	reload_shapes();
 	setupBulletCollisionObject(btBody);
 
 	set_mode(PhysicsServer::BODY_MODE_RIGID);
@@ -314,11 +315,9 @@ void RigidBodyBullet::destroy_kinematic_utilities() {
 	}
 }
 
-void RigidBodyBullet::main_shape_resetted() {
-	if (get_main_shape())
-		btBody->setCollisionShape(get_main_shape());
-	else
-		btBody->setCollisionShape(BulletPhysicsServer::get_empty_shape());
+void RigidBodyBullet::main_shape_changed() {
+	CRASH_COND(!get_main_shape())
+	btBody->setCollisionShape(get_main_shape());
 	set_continuous_collision_detection(is_continuous_collision_detection_enabled()); // Reset
 }
 
@@ -791,8 +790,8 @@ const btTransform &RigidBodyBullet::get_transform__bullet() const {
 	}
 }
 
-void RigidBodyBullet::on_shapes_changed() {
-	RigidCollisionObjectBullet::on_shapes_changed();
+void RigidBodyBullet::reload_shapes() {
+	RigidCollisionObjectBullet::reload_shapes();
 
 	const btScalar invMass = btBody->getInvMass();
 	const btScalar mass = invMass == 0 ? 0 : 1 / invMass;

modules/bullet/rigid_body_bullet.h

@@ -231,7 +231,7 @@ public:
 
 	_FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; }
 
-	virtual void main_shape_resetted();
+	virtual void main_shape_changed();
 	virtual void reload_body();
 	virtual void set_space(SpaceBullet *p_space);
 
@@ -302,7 +302,7 @@ public:
 	virtual void set_transform__bullet(const btTransform &p_global_transform);
 	virtual const btTransform &get_transform__bullet() const;
 
-	virtual void on_shapes_changed();
+	virtual void reload_shapes();
 
 	virtual void on_enter_area(AreaBullet *p_area);
 	virtual void on_exit_area(AreaBullet *p_area);

modules/bullet/shape_bullet.cpp

@@ -65,7 +65,8 @@ btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
 
 void ShapeBullet::notifyShapeChanged() {
 	for (Map<ShapeOwnerBullet *, int>::Element *E = owners.front(); E; E = E->next()) {
-		static_cast<ShapeOwnerBullet *>(E->key())->on_shape_changed(this);
+		ShapeOwnerBullet *owner = static_cast<ShapeOwnerBullet *>(E->key());
+		owner->shape_changed(owner->find_shape(this));
 	}
 }
 

modules/bullet/shape_owner_bullet.h

@@ -45,11 +45,10 @@ class CollisionObjectBullet;
 /// E.G. BodyShape is a child of this
 class ShapeOwnerBullet {
 public:
-	/// This is used to set new shape or replace existing
-	//virtual void _internal_replaceShape(btCollisionShape *p_old_shape, btCollisionShape *p_new_shape) = 0;
-	virtual void on_shape_changed(const ShapeBullet *const p_shape) = 0;
-	virtual void on_shapes_changed() = 0;
-	virtual void remove_shape(class ShapeBullet *p_shape) = 0;
+	virtual int find_shape(ShapeBullet *p_shape) const = 0;
+	virtual void shape_changed(int p_shape_index) = 0;
+	virtual void reload_shapes() = 0;
+	virtual void remove_shape_full(class ShapeBullet *p_shape) = 0;
 	virtual ~ShapeOwnerBullet() {}
 };
 #endif