Merge pull request #39726 from AndreaCatania/add_body_impr_physics

Optimized physics object spawn time

1 files changed, 55 insertions, 17 deletions

diff --git a/modules/bullet/space_bullet.cpp b/modules/bullet/space_bullet.cpp
index 0cafb30078..8204d7ed20 100644
--- a/modules/bullet/space_bullet.cpp
+++ b/modules/bullet/space_bullet.cpp
@@ -127,7 +127,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
 
 	btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale_abs(), p_margin);
 	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
+		shape->destroy_bt_shape(btShape);
 		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
 		return 0;
 	}
@@ -147,7 +147,7 @@ int BulletPhysicsDirectSpaceState::intersect_shape(const RID &p_shape, const Tra
 	btQuery.m_closestDistanceThreshold = 0;
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
-	bulletdelete(btConvex);
+	shape->destroy_bt_shape(btShape);
 
 	return btQuery.m_count;
 }
@@ -167,7 +167,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
 
 	btCollisionShape *btShape = shape->create_bt_shape(p_xform.basis.get_scale(), p_margin);
 	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
+		shape->destroy_bt_shape(btShape);
 		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
 		return false;
 	}
@@ -206,7 +206,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
 		r_closest_unsafe = 1.0f;
 	}
 
-	bulletdelete(bt_convex_shape);
+	shape->destroy_bt_shape(btShape);
 	return true; // Mean success
 }
 
@@ -221,7 +221,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
 
 	btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
 	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
+		shape->destroy_bt_shape(btShape);
 		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
 		return false;
 	}
@@ -242,7 +242,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
 	r_result_count = btQuery.m_count;
-	bulletdelete(btConvex);
+	shape->destroy_bt_shape(btShape);
 
 	return btQuery.m_count;
 }
@@ -253,7 +253,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
 
 	btCollisionShape *btShape = shape->create_bt_shape(p_shape_xform.basis.get_scale_abs(), p_margin);
 	if (!btShape->isConvex()) {
-		bulletdelete(btShape);
+		shape->destroy_bt_shape(btShape);
 		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
 		return false;
 	}
@@ -273,7 +273,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
 	btQuery.m_closestDistanceThreshold = 0;
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
-	bulletdelete(btConvex);
+	shape->destroy_bt_shape(btShape);
 
 	if (btQuery.m_collided) {
 		if (btCollisionObject::CO_RIGID_BODY == btQuery.m_rest_info_collision_object->getInternalType()) {
@@ -349,9 +349,11 @@ SpaceBullet::~SpaceBullet() {
 }
 
 void SpaceBullet::flush_queries() {
-	const btCollisionObjectArray &colObjArray = dynamicsWorld->getCollisionObjectArray();
-	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
-		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->dispatch_callbacks();
+	const int size = collision_objects.size();
+	CollisionObjectBullet **objects = collision_objects.ptrw();
+	for (int i = 0; i < size; i += 1) {
+		objects[i]->prepare_object_for_dispatch();
+		objects[i]->dispatch_callbacks();
 	}
 }
 
@@ -448,16 +450,30 @@ real_t SpaceBullet::get_param(PhysicsServer3D::SpaceParameter p_param) {
 }
 
 void SpaceBullet::add_area(AreaBullet *p_area) {
+#ifdef TOOLS_ENABLED
+	// This never happen, and there is no way for the user to trigger it.
+	// If in future a bug is introduced into this bullet integration and this
+	// function is called twice, the crash will notify the developer that will
+	// fix it even before do the eventual PR.
+	CRASH_COND(p_area->is_in_world);
+#endif
 	areas.push_back(p_area);
 	dynamicsWorld->addCollisionObject(p_area->get_bt_ghost(), p_area->get_collision_layer(), p_area->get_collision_mask());
+	p_area->is_in_world = true;
 }
 
 void SpaceBullet::remove_area(AreaBullet *p_area) {
-	areas.erase(p_area);
-	dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
+	if (p_area->is_in_world) {
+		areas.erase(p_area);
+		dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
+		p_area->is_in_world = false;
+	}
 }
 
 void SpaceBullet::reload_collision_filters(AreaBullet *p_area) {
+	if (p_area->is_in_world == false) {
+		return;
+	}
 	btGhostObject *ghost_object = p_area->get_bt_ghost();
 
 	btBroadphaseProxy *ghost_proxy = ghost_object->getBroadphaseHandle();
@@ -467,24 +483,46 @@ void SpaceBullet::reload_collision_filters(AreaBullet *p_area) {
 	dynamicsWorld->refreshBroadphaseProxy(ghost_object);
 }
 
+void SpaceBullet::register_collision_object(CollisionObjectBullet *p_object) {
+	collision_objects.push_back(p_object);
+}
+
+void SpaceBullet::unregister_collision_object(CollisionObjectBullet *p_object) {
+	collision_objects.erase(p_object);
+}
+
 void SpaceBullet::add_rigid_body(RigidBodyBullet *p_body) {
+#ifdef TOOLS_ENABLED
+	// This never happen, and there is no way for the user to trigger it.
+	// If in future a bug is introduced into this bullet integration and this
+	// function is called twice, the crash will notify the developer that will
+	// fix it even before do the eventual PR.
+	CRASH_COND(p_body->is_in_world);
+#endif
 	if (p_body->is_static()) {
 		dynamicsWorld->addCollisionObject(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
 	} else {
 		dynamicsWorld->addRigidBody(p_body->get_bt_rigid_body(), p_body->get_collision_layer(), p_body->get_collision_mask());
 		p_body->scratch_space_override_modificator();
 	}
+	p_body->is_in_world = true;
 }
 
 void SpaceBullet::remove_rigid_body(RigidBodyBullet *p_body) {
-	if (p_body->is_static()) {
-		dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
-	} else {
-		dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
+	if (p_body->is_in_world) {
+		if (p_body->is_static()) {
+			dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
+		} else {
+			dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
+		}
+		p_body->is_in_world = false;
 	}
 }
 
 void SpaceBullet::reload_collision_filters(RigidBodyBullet *p_body) {
+	if (p_body->is_in_world == false) {
+		return;
+	}
 	btRigidBody *rigid_body = p_body->get_bt_rigid_body();
 
 	btBroadphaseProxy *body_proxy = rigid_body->getBroadphaseProxy();