Merge pull request #42639 from AndreaCatania/revert_1

Reverted physics body spawn optimization #39726 #40252

modules/bullet/area_bullet.cpp

@@ -65,11 +65,14 @@ AreaBullet::~AreaBullet() {
 }
 
 void AreaBullet::dispatch_callbacks() {
-	RigidCollisionObjectBullet::dispatch_callbacks();
+	if (!isScratched) {
+		return;
+	}
+	isScratched = false;
 
 	// Reverse order because I've to remove EXIT objects
 	for (int i = overlappingObjects.size() - 1; 0 <= i; --i) {
-		OverlappingObjectData &otherObj = overlappingObjects[i];
+		OverlappingObjectData &otherObj = overlappingObjects.write[i];
 
 		switch (otherObj.state) {
 			case OVERLAP_STATE_ENTER:
@@ -109,9 +112,10 @@ void AreaBullet::call_event(CollisionObjectBullet *p_otherObject, PhysicsServer3
 }
 
 void AreaBullet::scratch() {
-	if (space != nullptr) {
-		space->add_to_pre_flush_queue(this);
+	if (isScratched) {
+		return;
 	}
+	isScratched = true;
 }
 
 void AreaBullet::clear_overlaps(bool p_notify) {
@@ -160,7 +164,7 @@ void AreaBullet::main_shape_changed() {
 	btGhost->setCollisionShape(get_main_shape());
 }
 
-void AreaBullet::do_reload_body() {
+void AreaBullet::reload_body() {
 	if (space) {
 		space->remove_area(this);
 		space->add_area(this);
@@ -169,25 +173,22 @@ void AreaBullet::do_reload_body() {
 
 void AreaBullet::set_space(SpaceBullet *p_space) {
 	// Clear the old space if there is one
-
 	if (space) {
 		clear_overlaps(false);
+		isScratched = false;
 
 		// Remove this object form the physics world
-		space->unregister_collision_object(this);
 		space->remove_area(this);
 	}
 
 	space = p_space;
 
 	if (space) {
-		space->register_collision_object(this);
-		reload_body();
-		scratch();
+		space->add_area(this);
 	}
 }
 
-void AreaBullet::do_reload_collision_filters() {
+void AreaBullet::on_collision_filters_change() {
 	if (space) {
 		space->reload_collision_filters(this);
 	}
@@ -201,13 +202,13 @@ void AreaBullet::add_overlap(CollisionObjectBullet *p_otherObject) {
 
 void AreaBullet::put_overlap_as_exit(int p_index) {
 	scratch();
-	overlappingObjects[p_index].state = OVERLAP_STATE_EXIT;
+	overlappingObjects.write[p_index].state = OVERLAP_STATE_EXIT;
 }
 
 void AreaBullet::put_overlap_as_inside(int p_index) {
 	// This check is required to be sure this body was inside
 	if (OVERLAP_STATE_DIRTY == overlappingObjects[p_index].state) {
-		overlappingObjects[p_index].state = OVERLAP_STATE_INSIDE;
+		overlappingObjects.write[p_index].state = OVERLAP_STATE_INSIDE;
 	}
 }
 

modules/bullet/area_bullet.h

@@ -32,7 +32,7 @@
 #define AREABULLET_H
 
 #include "collision_object_bullet.h"
-#include "core/local_vector.h"
+#include "core/vector.h"
 #include "servers/physics_server_3d.h"
 #include "space_bullet.h"
 
@@ -83,7 +83,7 @@ private:
 	Variant *call_event_res_ptr[5];
 
 	btGhostObject *btGhost;
-	LocalVector<OverlappingObjectData> overlappingObjects;
+	Vector<OverlappingObjectData> overlappingObjects;
 	bool monitorable = true;
 
 	PhysicsServer3D::AreaSpaceOverrideMode spOv_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
@@ -96,6 +96,8 @@ private:
 	real_t spOv_angularDump = 0.1;
 	int spOv_priority = 0;
 
+	bool isScratched = false;
+
 	InOutEventCallback eventsCallbacks[2];
 
 public:
@@ -137,11 +139,11 @@ 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_changed() override;
-	virtual void do_reload_body() override;
-	virtual void set_space(SpaceBullet *p_space) override;
+	virtual void main_shape_changed();
+	virtual void reload_body();
+	virtual void set_space(SpaceBullet *p_space);
 
-	virtual void dispatch_callbacks() override;
+	virtual void dispatch_callbacks();
 	void call_event(CollisionObjectBullet *p_otherObject, PhysicsServer3D::AreaBodyStatus p_status);
 	void set_on_state_change(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
 	void scratch();
@@ -150,9 +152,9 @@ public:
 	// Dispatch the callbacks and removes from overlapping list
 	void remove_overlap(CollisionObjectBullet *p_object, bool p_notify);
 
-	virtual void do_reload_collision_filters() override;
-	virtual void on_collision_checker_start() override {}
-	virtual void on_collision_checker_end() override { isTransformChanged = false; }
+	virtual void on_collision_filters_change();
+	virtual void on_collision_checker_start() {}
+	virtual void on_collision_checker_end() { isTransformChanged = false; }
 
 	void add_overlap(CollisionObjectBullet *p_otherObject);
 	void put_overlap_as_exit(int p_index);
@@ -164,8 +166,8 @@ public:
 	void set_event_callback(Type p_callbackObjectType, ObjectID p_id, const StringName &p_method);
 	bool has_event_callback(Type p_callbackObjectType);
 
-	virtual void on_enter_area(AreaBullet *p_area) override;
-	virtual void on_exit_area(AreaBullet *p_area) override;
+	virtual void on_enter_area(AreaBullet *p_area);
+	virtual void on_exit_area(AreaBullet *p_area);
 };
 
 #endif

modules/bullet/bullet_physics_server.h

@@ -52,7 +52,7 @@ class BulletPhysicsServer3D : public PhysicsServer3D {
 
 	bool active = true;
 	char active_spaces_count = 0;
-	LocalVector<SpaceBullet *> active_spaces;
+	Vector<SpaceBullet *> active_spaces;
 
 	mutable RID_PtrOwner<SpaceBullet> space_owner;
 	mutable RID_PtrOwner<ShapeBullet> shape_owner;

modules/bullet/collision_object_bullet.cpp

@@ -79,7 +79,7 @@ btTransform CollisionObjectBullet::ShapeWrapper::get_adjusted_transform() const
 }
 
 void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_scale) {
-	if (bt_shape == nullptr) {
+	if (!bt_shape) {
 		if (active) {
 			bt_shape = shape->create_bt_shape(scale * body_scale);
 		} else {
@@ -88,13 +88,6 @@ void CollisionObjectBullet::ShapeWrapper::claim_bt_shape(const btVector3 &body_s
 	}
 }
 
-void CollisionObjectBullet::ShapeWrapper::release_bt_shape() {
-	if (bt_shape != nullptr) {
-		shape->destroy_bt_shape(bt_shape);
-		bt_shape = nullptr;
-	}
-}
-
 CollisionObjectBullet::CollisionObjectBullet(Type p_type) :
 		RIDBullet(),
 		type(p_type) {}
@@ -165,22 +158,6 @@ bool CollisionObjectBullet::has_collision_exception(const CollisionObjectBullet
 	return !bt_collision_object->checkCollideWith(p_otherCollisionObject->bt_collision_object);
 }
 
-void CollisionObjectBullet::reload_body() {
-	needs_body_reload = true;
-}
-
-void CollisionObjectBullet::dispatch_callbacks() {}
-
-void CollisionObjectBullet::pre_process() {
-	if (needs_body_reload) {
-		do_reload_body();
-	} else if (needs_collision_filters_reload) {
-		do_reload_collision_filters();
-	}
-	needs_body_reload = false;
-	needs_collision_filters_reload = false;
-}
-
 void CollisionObjectBullet::set_collision_enabled(bool p_enabled) {
 	collisionsEnabled = p_enabled;
 	if (collisionsEnabled) {
@@ -254,7 +231,7 @@ void RigidCollisionObjectBullet::add_shape(ShapeBullet *p_shape, const Transform
 }
 
 void RigidCollisionObjectBullet::set_shape(int p_index, ShapeBullet *p_shape) {
-	ShapeWrapper &shp = shapes[p_index];
+	ShapeWrapper &shp = shapes.write[p_index];
 	shp.shape->remove_owner(this);
 	p_shape->add_owner(this);
 	shp.shape = p_shape;
@@ -316,7 +293,7 @@ void RigidCollisionObjectBullet::remove_all_shapes(bool p_permanentlyFromThisBod
 void RigidCollisionObjectBullet::set_shape_transform(int p_index, const Transform &p_transform) {
 	ERR_FAIL_INDEX(p_index, get_shape_count());
 
-	shapes[p_index].set_transform(p_transform);
+	shapes.write[p_index].set_transform(p_transform);
 	shape_changed(p_index);
 }
 
@@ -334,7 +311,7 @@ void RigidCollisionObjectBullet::set_shape_disabled(int p_index, bool p_disabled
 	if (shapes[p_index].active != p_disabled) {
 		return;
 	}
-	shapes[p_index].active = !p_disabled;
+	shapes.write[p_index].active = !p_disabled;
 	shape_changed(p_index);
 }
 
@@ -342,28 +319,16 @@ bool RigidCollisionObjectBullet::is_shape_disabled(int p_index) {
 	return !shapes[p_index].active;
 }
 
-void RigidCollisionObjectBullet::pre_process() {
-	if (need_shape_reload) {
-		do_reload_shapes();
-		need_shape_reload = false;
-	}
-	CollisionObjectBullet::pre_process();
-}
-
 void RigidCollisionObjectBullet::shape_changed(int p_shape_index) {
-	ShapeWrapper &shp = shapes[p_shape_index];
+	ShapeWrapper &shp = shapes.write[p_shape_index];
 	if (shp.bt_shape == mainShape) {
 		mainShape = nullptr;
 	}
-	shp.release_bt_shape();
+	bulletdelete(shp.bt_shape);
 	reload_shapes();
 }
 
 void RigidCollisionObjectBullet::reload_shapes() {
-	need_shape_reload = true;
-}
-
-void RigidCollisionObjectBullet::do_reload_shapes() {
 	if (mainShape && mainShape->isCompound()) {
 		// Destroy compound
 		bulletdelete(mainShape);
@@ -371,38 +336,41 @@ void RigidCollisionObjectBullet::do_reload_shapes() {
 
 	mainShape = nullptr;
 
+	ShapeWrapper *shpWrapper;
 	const int shape_count = shapes.size();
 
-	// Reset all shapes if required
+	// Reset shape if required
 	if (force_shape_reset) {
 		for (int i(0); i < shape_count; ++i) {
-			shapes[i].release_bt_shape();
+			shpWrapper = &shapes.write[i];
+			bulletdelete(shpWrapper->bt_shape);
 		}
 		force_shape_reset = false;
 	}
 
 	const btVector3 body_scale(get_bt_body_scale());
 
+	// Try to optimize by not using compound
 	if (1 == shape_count) {
-		// Is it possible to optimize by not using compound?
-		btTransform transform = shapes[0].get_adjusted_transform();
+		shpWrapper = &shapes.write[0];
+		btTransform transform = shpWrapper->get_adjusted_transform();
 		if (transform.getOrigin().isZero() && transform.getBasis() == transform.getBasis().getIdentity()) {
-			shapes[0].claim_bt_shape(body_scale);
-			mainShape = shapes[0].bt_shape;
+			shpWrapper->claim_bt_shape(body_scale);
+			mainShape = shpWrapper->bt_shape;
 			main_shape_changed();
-			// Nothing more to do
 			return;
 		}
 	}
 
-	// Optimization not possible use a compound shape.
+	// Optimization not possible use a compound shape
 	btCompoundShape *compoundShape = bulletnew(btCompoundShape(enableDynamicAabbTree, shape_count));
 
 	for (int i(0); i < shape_count; ++i) {
-		shapes[i].claim_bt_shape(body_scale);
-		btTransform scaled_shape_transform(shapes[i].get_adjusted_transform());
+		shpWrapper = &shapes.write[i];
+		shpWrapper->claim_bt_shape(body_scale);
+		btTransform scaled_shape_transform(shpWrapper->get_adjusted_transform());
 		scaled_shape_transform.getOrigin() *= body_scale;
-		compoundShape->addChildShape(scaled_shape_transform, shapes[i].bt_shape);
+		compoundShape->addChildShape(scaled_shape_transform, shpWrapper->bt_shape);
 	}
 
 	compoundShape->recalculateLocalAabb();
@@ -416,10 +384,10 @@ void RigidCollisionObjectBullet::body_scale_changed() {
 }
 
 void RigidCollisionObjectBullet::internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody) {
-	ShapeWrapper &shp = shapes[p_index];
+	ShapeWrapper &shp = shapes.write[p_index];
 	shp.shape->remove_owner(this, p_permanentlyFromThisBody);
 	if (shp.bt_shape == mainShape) {
 		mainShape = nullptr;
 	}
-	shp.release_bt_shape();
+	bulletdelete(shp.bt_shape);
 }

modules/bullet/collision_object_bullet.h

@@ -31,7 +31,6 @@
 #ifndef COLLISION_OBJECT_BULLET_H
 #define COLLISION_OBJECT_BULLET_H
 
-#include "core/local_vector.h"
 #include "core/math/transform.h"
 #include "core/math/vector3.h"
 #include "core/object.h"
@@ -71,12 +70,11 @@ public:
 
 	struct ShapeWrapper {
 		ShapeBullet *shape = nullptr;
+		btCollisionShape *bt_shape = nullptr;
 		btTransform transform;
 		btVector3 scale;
 		bool active = true;
-		btCollisionShape *bt_shape = nullptr;
 
-	public:
 		ShapeWrapper() {}
 
 		ShapeWrapper(ShapeBullet *p_shape, const btTransform &p_transform, bool p_active) :
@@ -109,7 +107,6 @@ public:
 		btTransform get_adjusted_transform() const;
 
 		void claim_bt_shape(const btVector3 &body_scale);
-		void release_bt_shape();
 	};
 
 protected:
@@ -127,19 +124,12 @@ protected:
 
 	VSet<RID> exceptions;
 
-	bool needs_body_reload = true;
-	bool needs_collision_filters_reload = true;
-
 	/// This array is used to know all areas where this Object is overlapped in
 	/// New area is added when overlap with new area (AreaBullet::addOverlap), then is removed when it exit (CollisionObjectBullet::onExitArea)
 	/// This array is used mainly to know which area hold the pointer of this object
-	LocalVector<AreaBullet *> areasOverlapped;
+	Vector<AreaBullet *> areasOverlapped;
 	bool isTransformChanged = false;
 
-public:
-	bool is_in_world = false;
-	bool is_in_flush_queue = false;
-
 public:
 	CollisionObjectBullet(Type p_type);
 	virtual ~CollisionObjectBullet();
@@ -174,7 +164,7 @@ public:
 	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) {
 		if (collisionLayer != p_layer) {
 			collisionLayer = p_layer;
-			needs_collision_filters_reload = true;
+			on_collision_filters_change();
 		}
 	}
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collisionLayer; }
@@ -182,32 +172,25 @@ public:
 	_FORCE_INLINE_ void set_collision_mask(uint32_t p_mask) {
 		if (collisionMask != p_mask) {
 			collisionMask = p_mask;
-			needs_collision_filters_reload = true;
+			on_collision_filters_change();
 		}
 	}
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collisionMask; }
 
-	virtual void do_reload_collision_filters() = 0;
+	virtual void on_collision_filters_change() = 0;
 
 	_FORCE_INLINE_ bool test_collision_mask(CollisionObjectBullet *p_other) const {
 		return collisionLayer & p_other->collisionMask || p_other->collisionLayer & collisionMask;
 	}
 
-	bool need_reload_body() const {
-		return needs_body_reload;
-	}
-
-	void reload_body();
-
-	virtual void do_reload_body() = 0;
+	virtual void reload_body() = 0;
 	virtual void set_space(SpaceBullet *p_space) = 0;
 	_FORCE_INLINE_ SpaceBullet *get_space() const { return space; }
 
 	virtual void on_collision_checker_start() = 0;
 	virtual void on_collision_checker_end() = 0;
 
-	virtual void dispatch_callbacks();
-	virtual void pre_process();
+	virtual void dispatch_callbacks() = 0;
 
 	void set_collision_enabled(bool p_enabled);
 	bool is_collisions_response_enabled();
@@ -231,15 +214,14 @@ public:
 class RigidCollisionObjectBullet : public CollisionObjectBullet, public ShapeOwnerBullet {
 protected:
 	btCollisionShape *mainShape = nullptr;
-	LocalVector<ShapeWrapper> shapes;
-	bool need_shape_reload = true;
+	Vector<ShapeWrapper> shapes;
 
 public:
 	RigidCollisionObjectBullet(Type p_type) :
 			CollisionObjectBullet(p_type) {}
 	~RigidCollisionObjectBullet();
 
-	_FORCE_INLINE_ const LocalVector<ShapeWrapper> &get_shapes_wrappers() const { return shapes; }
+	_FORCE_INLINE_ const Vector<ShapeWrapper> &get_shapes_wrappers() const { return shapes; }
 
 	_FORCE_INLINE_ btCollisionShape *get_main_shape() const { return mainShape; }
 
@@ -250,9 +232,9 @@ public:
 	ShapeBullet *get_shape(int p_index) const;
 	btCollisionShape *get_bt_shape(int p_index) const;
 
-	virtual int find_shape(ShapeBullet *p_shape) const override;
+	int find_shape(ShapeBullet *p_shape) const;
 
-	virtual void remove_shape_full(ShapeBullet *p_shape) override;
+	virtual void remove_shape_full(ShapeBullet *p_shape);
 	void remove_shape_full(int p_index);
 	void remove_all_shapes(bool p_permanentlyFromThisBody = false, bool p_force_not_reload = false);
 
@@ -264,15 +246,11 @@ public:
 	void set_shape_disabled(int p_index, bool p_disabled);
 	bool is_shape_disabled(int p_index);
 
-	virtual void pre_process() override;
-
-	virtual void shape_changed(int p_shape_index) override;
-	virtual void reload_shapes() override;
-	bool need_reload_shapes() const { return need_shape_reload; }
-	virtual void do_reload_shapes();
+	virtual void shape_changed(int p_shape_index);
+	virtual void reload_shapes();
 
 	virtual void main_shape_changed() = 0;
-	virtual void body_scale_changed() override;
+	virtual void body_scale_changed();
 
 private:
 	void internal_shape_destroy(int p_index, bool p_permanentlyFromThisBody = false);

modules/bullet/rigid_body_bullet.cpp

@@ -51,7 +51,9 @@
 BulletPhysicsDirectBodyState3D *BulletPhysicsDirectBodyState3D::singleton = nullptr;
 
 Vector3 BulletPhysicsDirectBodyState3D::get_total_gravity() const {
-	return body->total_gravity;
+	Vector3 gVec;
+	B_TO_G(body->btBody->getGravity(), gVec);
+	return gVec;
 }
 
 float BulletPhysicsDirectBodyState3D::get_total_angular_damp() const {
@@ -181,7 +183,7 @@ int BulletPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx
 }
 
 Vector3 BulletPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
-	RigidBodyBullet::CollisionData &colDat = body->collisions[p_contact_idx];
+	RigidBodyBullet::CollisionData &colDat = body->collisions.write[p_contact_idx];
 
 	btVector3 hitLocation;
 	G_TO_B(colDat.hitLocalLocation, hitLocation);
@@ -211,7 +213,7 @@ void RigidBodyBullet::KinematicUtilities::setSafeMargin(btScalar p_margin) {
 }
 
 void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
-	const LocalVector<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(owner->get_shapes_wrappers());
+	const Vector<CollisionObjectBullet::ShapeWrapper> &shapes_wrappers(owner->get_shapes_wrappers());
 	const int shapes_count = shapes_wrappers.size();
 
 	just_delete_shapes(shapes_count);
@@ -226,8 +228,8 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 			continue;
 		}
 
-		shapes[i].transform = shape_wrapper->transform;
-		shapes[i].transform.getOrigin() *= owner_scale;
+		shapes.write[i].transform = shape_wrapper->transform;
+		shapes.write[i].transform.getOrigin() *= owner_scale;
 		switch (shape_wrapper->shape->get_type()) {
 			case PhysicsServer3D::SHAPE_SPHERE:
 			case PhysicsServer3D::SHAPE_BOX:
@@ -235,11 +237,11 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 			case PhysicsServer3D::SHAPE_CYLINDER:
 			case PhysicsServer3D::SHAPE_CONVEX_POLYGON:
 			case PhysicsServer3D::SHAPE_RAY: {
-				shapes[i].shape = static_cast<btConvexShape *>(shape_wrapper->shape->internal_create_bt_shape(owner_scale * shape_wrapper->scale, safe_margin));
+				shapes.write[i].shape = static_cast<btConvexShape *>(shape_wrapper->shape->create_bt_shape(owner_scale * shape_wrapper->scale, safe_margin));
 			} break;
 			default:
 				WARN_PRINT("This shape is not supported for kinematic collision.");
-				shapes[i].shape = nullptr;
+				shapes.write[i].shape = nullptr;
 		}
 	}
 }
@@ -247,7 +249,7 @@ void RigidBodyBullet::KinematicUtilities::copyAllOwnerShapes() {
 void RigidBodyBullet::KinematicUtilities::just_delete_shapes(int new_size) {
 	for (int i = shapes.size() - 1; 0 <= i; --i) {
 		if (shapes[i].shape) {
-			bulletdelete(shapes[i].shape);
+			bulletdelete(shapes.write[i].shape);
 		}
 	}
 	shapes.resize(new_size);
@@ -269,8 +271,8 @@ RigidBodyBullet::RigidBodyBullet() :
 	reload_axis_lock();
 
 	areasWhereIam.resize(maxAreasWhereIam);
-	for (uint32_t i = 0; i < areasWhereIam.size(); i += 1) {
-		areasWhereIam[i] = nullptr;
+	for (int i = areasWhereIam.size() - 1; 0 <= i; --i) {
+		areasWhereIam.write[i] = nullptr;
 	}
 	btBody->setSleepingThresholds(0.2, 0.2);
 
@@ -305,7 +307,7 @@ void RigidBodyBullet::main_shape_changed() {
 	set_continuous_collision_detection(is_continuous_collision_detection_enabled()); // Reset
 }
 
-void RigidBodyBullet::do_reload_body() {
+void RigidBodyBullet::reload_body() {
 	if (space) {
 		space->remove_rigid_body(this);
 		if (get_main_shape()) {
@@ -324,24 +326,23 @@ void RigidBodyBullet::set_space(SpaceBullet *p_space) {
 		assert_no_constraints();
 
 		// Remove this object form the physics world
-		space->unregister_collision_object(this);
 		space->remove_rigid_body(this);
 	}
 
 	space = p_space;
 
 	if (space) {
-		space->register_collision_object(this);
-		reload_body();
-		space->add_to_flush_queue(this);
+		space->add_rigid_body(this);
 	}
 }
 
 void RigidBodyBullet::dispatch_callbacks() {
-	RigidCollisionObjectBullet::dispatch_callbacks();
-
 	/// The check isFirstTransformChanged is necessary in order to call integrated forces only when the first transform is sent
 	if ((btBody->isKinematicObject() || btBody->isActive() || previousActiveState != btBody->isActive()) && force_integration_callback && can_integrate_forces) {
+		if (omit_forces_integration) {
+			btBody->clearForces();
+		}
+
 		BulletPhysicsDirectBodyState3D *bodyDirect = BulletPhysicsDirectBodyState3D::get_singleton(this);
 
 		Variant variantBodyDirect = bodyDirect;
@@ -359,22 +360,16 @@ void RigidBodyBullet::dispatch_callbacks() {
 		}
 	}
 
-	previousActiveState = btBody->isActive();
-}
-
-void RigidBodyBullet::pre_process() {
-	RigidCollisionObjectBullet::pre_process();
-
 	if (isScratchedSpaceOverrideModificator || 0 < countGravityPointSpaces) {
 		isScratchedSpaceOverrideModificator = false;
 		reload_space_override_modificator();
 	}
 
-	if (is_active()) {
-		/// Lock axis
-		btBody->setLinearVelocity(btBody->getLinearVelocity() * btBody->getLinearFactor());
-		btBody->setAngularVelocity(btBody->getAngularVelocity() * btBody->getAngularFactor());
-	}
+	/// Lock axis
+	btBody->setLinearVelocity(btBody->getLinearVelocity() * btBody->getLinearFactor());
+	btBody->setAngularVelocity(btBody->getAngularVelocity() * btBody->getAngularFactor());
+
+	previousActiveState = btBody->isActive();
 }
 
 void RigidBodyBullet::set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata) {
@@ -395,7 +390,7 @@ void RigidBodyBullet::scratch_space_override_modificator() {
 	isScratchedSpaceOverrideModificator = true;
 }
 
-void RigidBodyBullet::do_reload_collision_filters() {
+void RigidBodyBullet::on_collision_filters_change() {
 	if (space) {
 		space->reload_collision_filters(this);
 	}
@@ -408,15 +403,14 @@ void RigidBodyBullet::on_collision_checker_start() {
 	collisionsCount = 0;
 
 	// Swap array
-	SWAP(prev_collision_traces, curr_collision_traces);
+	Vector<RigidBodyBullet *> *s = prev_collision_traces;
+	prev_collision_traces = curr_collision_traces;
+	curr_collision_traces = s;
 }
 
 void RigidBodyBullet::on_collision_checker_end() {
 	// Always true if active and not a static or kinematic body
 	isTransformChanged = btBody->isActive() && !btBody->isStaticOrKinematicObject();
-	if (isTransformChanged && space != nullptr) {
-		space->add_to_flush_queue(this);
-	}
 }
 
 bool RigidBodyBullet::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) {
@@ -424,7 +418,7 @@ bool RigidBodyBullet::add_collision_object(RigidBodyBullet *p_otherObject, const
 		return false;
 	}
 
-	CollisionData &cd = collisions[collisionsCount];
+	CollisionData &cd = collisions.write[collisionsCount];
 	cd.hitLocalLocation = p_hitLocalLocation;
 	cd.otherObject = p_otherObject;
 	cd.hitWorldLocation = p_hitWorldLocation;
@@ -433,7 +427,7 @@ 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)[collisionsCount] = p_otherObject;
+	curr_collision_traces->write[collisionsCount] = p_otherObject;
 
 	++collisionsCount;
 	return true;
@@ -468,7 +462,6 @@ bool RigidBodyBullet::is_active() const {
 
 void RigidBodyBullet::set_omit_forces_integration(bool p_omit) {
 	omit_forces_integration = p_omit;
-	scratch_space_override_modificator();
 }
 
 void RigidBodyBullet::set_param(PhysicsServer3D::BodyParameter p_param, real_t p_value) {
@@ -811,8 +804,8 @@ const btTransform &RigidBodyBullet::get_transform__bullet() const {
 	}
 }
 
-void RigidBodyBullet::do_reload_shapes() {
-	RigidCollisionObjectBullet::do_reload_shapes();
+void RigidBodyBullet::reload_shapes() {
+	RigidCollisionObjectBullet::reload_shapes();
 
 	const btScalar invMass = btBody->getInvMass();
 	const btScalar mass = invMass == 0 ? 0 : 1 / invMass;
@@ -844,15 +837,15 @@ void RigidBodyBullet::on_enter_area(AreaBullet *p_area) {
 	for (int i = 0; i < areaWhereIamCount; ++i) {
 		if (nullptr == areasWhereIam[i]) {
 			// This area has the highest priority
-			areasWhereIam[i] = p_area;
+			areasWhereIam.write[i] = p_area;
 			break;
 		} else {
 			if (areasWhereIam[i]->get_spOv_priority() > p_area->get_spOv_priority()) {
 				// The position was found, just shift all elements
 				for (int j = areaWhereIamCount; j > i; j--) {
-					areasWhereIam[j] = areasWhereIam[j - 1];
+					areasWhereIam.write[j] = areasWhereIam[j - 1];
 				}
-				areasWhereIam[i] = p_area;
+				areasWhereIam.write[i] = p_area;
 				break;
 			}
 		}
@@ -876,7 +869,7 @@ void RigidBodyBullet::on_exit_area(AreaBullet *p_area) {
 		if (p_area == areasWhereIam[i]) {
 			// The area was found, just shift down all elements
 			for (int j = i; j < areaWhereIamCount; ++j) {
-				areasWhereIam[j] = areasWhereIam[j + 1];
+				areasWhereIam.write[j] = areasWhereIam[j + 1];
 			}
 			wasTheAreaFound = true;
 			break;
@@ -889,7 +882,7 @@ void RigidBodyBullet::on_exit_area(AreaBullet *p_area) {
 		}
 
 		--areaWhereIamCount;
-		areasWhereIam[areaWhereIamCount] = nullptr; // Even if this is not required, I clear the last element to be safe
+		areasWhereIam.write[areaWhereIamCount] = nullptr; // Even if this is not required, I clear the last element to be safe
 		if (PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED != p_area->get_spOv_mode()) {
 			scratch_space_override_modificator();
 		}
@@ -901,31 +894,36 @@ void RigidBodyBullet::reload_space_override_modificator() {
 		return;
 	}
 
-	Vector3 newGravity;
+	Vector3 newGravity(0.0, 0.0, 0.0);
 	real_t newLinearDamp = MAX(0.0, linearDamp);
 	real_t newAngularDamp = MAX(0.0, angularDamp);
 
+	AreaBullet *currentArea;
+	// Variable used to calculate new gravity for gravity point areas, it is pointed by currentGravity pointer
+	Vector3 support_gravity(0, 0, 0);
+
 	bool stopped = false;
-	for (int i = 0; i < areaWhereIamCount && !stopped; i += 1) {
-		AreaBullet *currentArea = areasWhereIam[i];
+	for (int i = areaWhereIamCount - 1; (0 <= i) && !stopped; --i) {
+		currentArea = areasWhereIam[i];
 
 		if (!currentArea || PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED == currentArea->get_spOv_mode()) {
 			continue;
 		}
 
-		Vector3 support_gravity;
-
 		/// Here is calculated the gravity
 		if (currentArea->is_spOv_gravityPoint()) {
 			/// It calculates the direction of new gravity
 			support_gravity = currentArea->get_transform().xform(currentArea->get_spOv_gravityVec()) - get_transform().get_origin();
-
-			const real_t distanceMag = support_gravity.length();
+			real_t distanceMag = support_gravity.length();
 			// Normalized in this way to avoid the double call of function "length()"
 			if (distanceMag == 0) {
-				support_gravity = Vector3();
+				support_gravity.x = 0;
+				support_gravity.y = 0;
+				support_gravity.z = 0;
 			} else {
-				support_gravity /= distanceMag;
+				support_gravity.x /= distanceMag;
+				support_gravity.y /= distanceMag;
+				support_gravity.z /= distanceMag;
 			}
 
 			/// Here is calculated the final gravity
@@ -987,17 +985,10 @@ void RigidBodyBullet::reload_space_override_modificator() {
 		newAngularDamp += space->get_angular_damp();
 	}
 
-	total_gravity = newGravity;
-
-	if (omit_forces_integration) {
-		// Custom behaviour.
-		btBody->setGravity(btVector3(0, 0, 0));
-	} else {
-		btVector3 newBtGravity;
-		G_TO_B(newGravity * gravity_scale, newBtGravity);
-		btBody->setGravity(newBtGravity);
-	}
+	btVector3 newBtGravity;
+	G_TO_B(newGravity * gravity_scale, newBtGravity);
 
+	btBody->setGravity(newBtGravity);
 	btBody->setDamping(newLinearDamp, newAngularDamp);
 }
 

modules/bullet/rigid_body_bullet.h

@@ -171,7 +171,7 @@ public:
 	struct KinematicUtilities {
 		RigidBodyBullet *owner;
 		btScalar safe_margin;
-		LocalVector<KinematicShape> shapes;
+		Vector<KinematicShape> shapes;
 
 		KinematicUtilities(RigidBodyBullet *p_owner);
 		~KinematicUtilities();
@@ -193,7 +193,6 @@ private:
 	PhysicsServer3D::BodyMode mode;
 	GodotMotionState *godotMotionState;
 	btRigidBody *btBody;
-	Vector3 total_gravity;
 	uint16_t locked_axis = 0;
 	real_t mass = 1;
 	real_t gravity_scale = 1;
@@ -203,18 +202,18 @@ private:
 	bool omit_forces_integration = false;
 	bool can_integrate_forces = false;
 
-	LocalVector<CollisionData> collisions;
-	LocalVector<RigidBodyBullet *> collision_traces_1;
-	LocalVector<RigidBodyBullet *> collision_traces_2;
-	LocalVector<RigidBodyBullet *> *prev_collision_traces;
-	LocalVector<RigidBodyBullet *> *curr_collision_traces;
+	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
-	uint32_t maxCollisionsDetection = 0;
-	uint32_t collisionsCount = 0;
-	uint32_t prev_collision_count = 0;
+	int maxCollisionsDetection = 0;
+	int collisionsCount = 0;
+	int prev_collision_count = 0;
 
-	LocalVector<AreaBullet *> areasWhereIam;
+	Vector<AreaBullet *> areasWhereIam;
 	// these parameters are used to avoid vector resize
 	int maxAreasWhereIam = 10;
 	int areaWhereIamCount = 0;
@@ -236,20 +235,21 @@ public:
 
 	_FORCE_INLINE_ btRigidBody *get_bt_rigid_body() { return btBody; }
 
-	virtual void main_shape_changed() override;
-	virtual void do_reload_body() override;
-	virtual void set_space(SpaceBullet *p_space) override;
+	virtual void main_shape_changed();
+	virtual void reload_body();
+	virtual void set_space(SpaceBullet *p_space);
 
-	virtual void dispatch_callbacks() override;
-	virtual void pre_process() override;
+	virtual void dispatch_callbacks();
 	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
 	void scratch_space_override_modificator();
 
-	virtual void do_reload_collision_filters() override;
-	virtual void on_collision_checker_start() override;
-	virtual void on_collision_checker_end() override;
+	virtual void on_collision_filters_change();
+	virtual void on_collision_checker_start();
+	virtual void on_collision_checker_end();
+
+	void set_max_collisions_detection(int p_maxCollisionsDetection) {
+		ERR_FAIL_COND(0 > p_maxCollisionsDetection);
 
-	void set_max_collisions_detection(uint32_t p_maxCollisionsDetection) {
 		maxCollisionsDetection = p_maxCollisionsDetection;
 
 		collisions.resize(p_maxCollisionsDetection);
@@ -312,19 +312,19 @@ public:
 	void set_angular_velocity(const Vector3 &p_velocity);
 	Vector3 get_angular_velocity() const;
 
-	virtual void set_transform__bullet(const btTransform &p_global_transform) override;
-	virtual const btTransform &get_transform__bullet() const override;
+	virtual void set_transform__bullet(const btTransform &p_global_transform);
+	virtual const btTransform &get_transform__bullet() const;
 
-	virtual void do_reload_shapes() override;
+	virtual void reload_shapes();
 
-	virtual void on_enter_area(AreaBullet *p_area) override;
-	virtual void on_exit_area(AreaBullet *p_area) override;
+	virtual void on_enter_area(AreaBullet *p_area);
+	virtual void on_exit_area(AreaBullet *p_area);
 	void reload_space_override_modificator();
 
 	/// Kinematic
 	void reload_kinematic_shapes();
 
-	virtual void notify_transform_changed() override;
+	virtual void notify_transform_changed();
 
 private:
 	void _internal_set_mass(real_t p_mass);

modules/bullet/shape_bullet.cpp

@@ -46,15 +46,9 @@
 	@author AndreaCatania
 */
 
-ShapeBullet::ShapeBullet() {
-}
+ShapeBullet::ShapeBullet() {}
 
-ShapeBullet::~ShapeBullet() {
-	if (default_shape != nullptr) {
-		bulletdelete(default_shape);
-		default_shape = nullptr;
-	}
-}
+ShapeBullet::~ShapeBullet() {}
 
 btCollisionShape *ShapeBullet::create_bt_shape(const Vector3 &p_implicit_scale, real_t p_extra_edge) {
 	btVector3 s;
@@ -62,22 +56,6 @@ btCollisionShape *ShapeBullet::create_bt_shape(const Vector3 &p_implicit_scale,
 	return create_bt_shape(s, p_extra_edge);
 }
 
-btCollisionShape *ShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	if (p_extra_edge == 0.0 && (p_implicit_scale - btVector3(1, 1, 1)).length2() <= CMP_EPSILON) {
-		return default_shape;
-	}
-
-	return internal_create_bt_shape(p_implicit_scale, p_extra_edge);
-}
-
-void ShapeBullet::destroy_bt_shape(btCollisionShape *p_shape) const {
-	if (p_shape != default_shape && p_shape != old_default_shape) {
-		if (likely(p_shape != nullptr)) {
-			bulletdelete(p_shape);
-		}
-	}
-}
-
 btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
 	p_btShape->setUserPointer(const_cast<ShapeBullet *>(this));
 	p_btShape->setMargin(margin);
@@ -85,21 +63,10 @@ btCollisionShape *ShapeBullet::prepare(btCollisionShape *p_btShape) const {
 }
 
 void ShapeBullet::notifyShapeChanged() {
-	// Store the old shape ptr so to not lose the reference pointer.
-	old_default_shape = default_shape;
-	// Create the new default shape with the new data.
-	default_shape = internal_create_bt_shape(btVector3(1, 1, 1));
-
 	for (Map<ShapeOwnerBullet *, int>::Element *E = owners.front(); E; E = E->next()) {
 		ShapeOwnerBullet *owner = static_cast<ShapeOwnerBullet *>(E->key());
 		owner->shape_changed(owner->find_shape(this));
 	}
-
-	if (old_default_shape) {
-		// At this point now one has the old default shape; just delete it.
-		bulletdelete(old_default_shape);
-		old_default_shape = nullptr;
-	}
 }
 
 void ShapeBullet::add_owner(ShapeOwnerBullet *p_owner) {
@@ -219,7 +186,7 @@ void PlaneShapeBullet::setup(const Plane &p_plane) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *PlaneShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+btCollisionShape *PlaneShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
 	btVector3 btPlaneNormal;
 	G_TO_B(plane.normal, btPlaneNormal);
 	return prepare(PlaneShapeBullet::create_shape_plane(btPlaneNormal, plane.d));
@@ -247,7 +214,7 @@ void SphereShapeBullet::setup(real_t p_radius) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *SphereShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+btCollisionShape *SphereShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
 	return prepare(ShapeBullet::create_shape_sphere(radius * p_implicit_scale[0] + p_extra_edge));
 }
 
@@ -274,7 +241,7 @@ void BoxShapeBullet::setup(const Vector3 &p_half_extents) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *BoxShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+btCollisionShape *BoxShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
 	return prepare(ShapeBullet::create_shape_box((half_extents * p_implicit_scale) + btVector3(p_extra_edge, p_extra_edge, p_extra_edge)));
 }
 
@@ -307,8 +274,8 @@ void CapsuleShapeBullet::setup(real_t p_height, real_t p_radius) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *CapsuleShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
-	return prepare(ShapeBullet::create_shape_capsule(radius * p_implicit_scale[0] + p_extra_edge, height * p_implicit_scale[1]));
+btCollisionShape *CapsuleShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+	return prepare(ShapeBullet::create_shape_capsule(radius * p_implicit_scale[0] + p_extra_edge, height * p_implicit_scale[1] + p_extra_edge));
 }
 
 /* Cylinder */
@@ -340,7 +307,7 @@ void CylinderShapeBullet::setup(real_t p_height, real_t p_radius) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *CylinderShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
+btCollisionShape *CylinderShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin) {
 	return prepare(ShapeBullet::create_shape_cylinder(radius * p_implicit_scale[0] + p_margin, height * p_implicit_scale[1] + p_margin));
 }
 
@@ -382,7 +349,7 @@ void ConvexPolygonShapeBullet::setup(const Vector<Vector3> &p_vertices) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *ConvexPolygonShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+btCollisionShape *ConvexPolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
 	if (!vertices.size()) {
 		// This is necessary since 0 vertices
 		return prepare(ShapeBullet::create_shape_empty());
@@ -464,7 +431,7 @@ void ConcavePolygonShapeBullet::setup(Vector<Vector3> p_faces) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *ConcavePolygonShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+btCollisionShape *ConcavePolygonShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
 	btCollisionShape *cs = ShapeBullet::create_shape_concave(meshShape);
 	if (!cs) {
 		// This is necessary since if 0 faces the creation of concave return null
@@ -591,7 +558,7 @@ void HeightMapShapeBullet::setup(Vector<real_t> &p_heights, int p_width, int p_d
 	notifyShapeChanged();
 }
 
-btCollisionShape *HeightMapShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+btCollisionShape *HeightMapShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
 	btCollisionShape *cs(ShapeBullet::create_shape_height_field(heights, width, depth, min_height, max_height));
 	cs->setLocalScaling(p_implicit_scale);
 	prepare(cs);
@@ -624,6 +591,6 @@ void RayShapeBullet::setup(real_t p_length, bool p_slips_on_slope) {
 	notifyShapeChanged();
 }
 
-btCollisionShape *RayShapeBullet::internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
+btCollisionShape *RayShapeBullet::create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge) {
 	return prepare(ShapeBullet::create_shape_ray(length * p_implicit_scale[1] + p_extra_edge, slips_on_slope));
 }

modules/bullet/shape_bullet.h

@@ -53,10 +53,6 @@ class ShapeBullet : public RIDBullet {
 	Map<ShapeOwnerBullet *, int> owners;
 	real_t margin = 0.04;
 
-	// Contains the default shape.
-	btCollisionShape *default_shape = nullptr;
-	btCollisionShape *old_default_shape = nullptr;
-
 protected:
 	/// return self
 	btCollisionShape *prepare(btCollisionShape *p_btShape) const;
@@ -67,11 +63,7 @@ public:
 	virtual ~ShapeBullet();
 
 	btCollisionShape *create_bt_shape(const Vector3 &p_implicit_scale, real_t p_extra_edge = 0);
-	btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
-
-	void destroy_bt_shape(btCollisionShape *p_shape) const;
-
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0) = 0;
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0) = 0;
 
 	void add_owner(ShapeOwnerBullet *p_owner);
 	void remove_owner(ShapeOwnerBullet *p_owner, bool p_permanentlyFromThisBody = false);
@@ -110,7 +102,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(const Plane &p_plane);
@@ -126,7 +118,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(real_t p_radius);
@@ -142,7 +134,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(const Vector3 &p_half_extents);
@@ -160,7 +152,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(real_t p_height, real_t p_radius);
@@ -178,7 +170,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_margin = 0);
 
 private:
 	void setup(real_t p_height, real_t p_radius);
@@ -194,7 +186,7 @@ public:
 	void get_vertices(Vector<Vector3> &out_vertices);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(const Vector<Vector3> &p_vertices);
@@ -212,7 +204,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(Vector<Vector3> p_faces);
@@ -231,7 +223,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(Vector<real_t> &p_heights, int p_width, int p_depth, real_t p_min_height, real_t p_max_height);
@@ -247,7 +239,7 @@ public:
 	virtual void set_data(const Variant &p_data);
 	virtual Variant get_data() const;
 	virtual PhysicsServer3D::ShapeType get_type() const;
-	virtual btCollisionShape *internal_create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
+	virtual btCollisionShape *create_bt_shape(const btVector3 &p_implicit_scale, real_t p_extra_edge = 0);
 
 private:
 	void setup(real_t p_length, bool p_slips_on_slope);

modules/bullet/soft_body_bullet.cpp

@@ -41,7 +41,7 @@ SoftBodyBullet::SoftBodyBullet() :
 SoftBodyBullet::~SoftBodyBullet() {
 }
 
-void SoftBodyBullet::do_reload_body() {
+void SoftBodyBullet::reload_body() {
 	if (space) {
 		space->remove_soft_body(this);
 		space->add_soft_body(this);
@@ -51,15 +51,13 @@ void SoftBodyBullet::do_reload_body() {
 void SoftBodyBullet::set_space(SpaceBullet *p_space) {
 	if (space) {
 		isScratched = false;
-		space->unregister_collision_object(this);
 		space->remove_soft_body(this);
 	}
 
 	space = p_space;
 
 	if (space) {
-		space->register_collision_object(this);
-		reload_body();
+		space->add_soft_body(this);
 	}
 }
 
@@ -346,14 +344,14 @@ void SoftBodyBullet::set_trimesh_body_shape(Vector<int> p_indices, Vector<Vector
 					indices_table.push_back(Vector<int>());
 				}
 
-				indices_table[vertex_id].push_back(vs_vertex_index);
+				indices_table.write[vertex_id].push_back(vs_vertex_index);
 				vs_indices_to_physics_table.push_back(vertex_id);
 			}
 		}
 
 		const int indices_map_size(indices_table.size());
 
-		LocalVector<btScalar> bt_vertices;
+		Vector<btScalar> bt_vertices;
 
 		{ // Parse vertices to bullet
 
@@ -361,13 +359,13 @@ void SoftBodyBullet::set_trimesh_body_shape(Vector<int> p_indices, Vector<Vector
 			const Vector3 *p_vertices_read = p_vertices.ptr();
 
 			for (int i = 0; i < indices_map_size; ++i) {
-				bt_vertices[3 * i + 0] = p_vertices_read[indices_table[i][0]].x;
-				bt_vertices[3 * i + 1] = p_vertices_read[indices_table[i][0]].y;
-				bt_vertices[3 * i + 2] = p_vertices_read[indices_table[i][0]].z;
+				bt_vertices.write[3 * i + 0] = p_vertices_read[indices_table[i][0]].x;
+				bt_vertices.write[3 * i + 1] = p_vertices_read[indices_table[i][0]].y;
+				bt_vertices.write[3 * i + 2] = p_vertices_read[indices_table[i][0]].z;
 			}
 		}
 
-		LocalVector<int> bt_triangles;
+		Vector<int> bt_triangles;
 		const int triangles_size(p_indices.size() / 3);
 
 		{ // Parse indices
@@ -377,9 +375,9 @@ void SoftBodyBullet::set_trimesh_body_shape(Vector<int> p_indices, Vector<Vector
 			const int *p_indices_read = p_indices.ptr();
 
 			for (int i = 0; i < triangles_size; ++i) {
-				bt_triangles[3 * i + 0] = vs_indices_to_physics_table[p_indices_read[3 * i + 2]];
-				bt_triangles[3 * i + 1] = vs_indices_to_physics_table[p_indices_read[3 * i + 1]];
-				bt_triangles[3 * i + 2] = vs_indices_to_physics_table[p_indices_read[3 * i + 0]];
+				bt_triangles.write[3 * i + 0] = vs_indices_to_physics_table[p_indices_read[3 * i + 2]];
+				bt_triangles.write[3 * i + 1] = vs_indices_to_physics_table[p_indices_read[3 * i + 1]];
+				bt_triangles.write[3 * i + 2] = vs_indices_to_physics_table[p_indices_read[3 * i + 0]];
 			}
 		}
 

modules/bullet/soft_body_bullet.h

@@ -32,6 +32,7 @@
 #define SOFT_BODY_BULLET_H
 
 #include "collision_object_bullet.h"
+#include "scene/resources/material.h" // TODO remove this please
 
 #ifdef None
 /// This is required to remove the macro None defined by x11 compiler because this word "None" is used internally by Bullet
@@ -57,7 +58,7 @@
 class SoftBodyBullet : public CollisionObjectBullet {
 private:
 	btSoftBody *bt_soft_body = nullptr;
-	LocalVector<Vector<int>> indices_table;
+	Vector<Vector<int>> indices_table;
 	btSoftBody::Material *mat0; // This is just a copy of pointer managed by btSoftBody
 	bool isScratched = false;
 
@@ -72,7 +73,7 @@ private:
 	real_t pose_matching_coefficient = 0.; // [0,1]
 	real_t damping_coefficient = 0.01; // [0,1]
 	real_t drag_coefficient = 0.; // [0,1]
-	LocalVector<int> pinned_nodes;
+	Vector<int> pinned_nodes;
 
 	// Other property to add
 	//btScalar				kVC;			// Volume conversation coefficient [0,+inf]
@@ -86,14 +87,15 @@ public:
 	SoftBodyBullet();
 	~SoftBodyBullet();
 
-	virtual void do_reload_body() override;
-	virtual void set_space(SpaceBullet *p_space) override;
+	virtual void reload_body();
+	virtual void set_space(SpaceBullet *p_space);
 
-	virtual void do_reload_collision_filters() override {}
-	virtual void on_collision_checker_start() override {}
-	virtual void on_collision_checker_end() override {}
-	virtual void on_enter_area(AreaBullet *p_area) override;
-	virtual void on_exit_area(AreaBullet *p_area) override;
+	virtual void dispatch_callbacks() {}
+	virtual void on_collision_filters_change() {}
+	virtual void on_collision_checker_start() {}
+	virtual void on_collision_checker_end() {}
+	virtual void on_enter_area(AreaBullet *p_area);
+	virtual void on_exit_area(AreaBullet *p_area);
 
 	_FORCE_INLINE_ btSoftBody *get_bt_soft_body() const { return bt_soft_body; }
 

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()) {
-		shape->destroy_bt_shape(btShape);
+		bulletdelete(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);
 
-	shape->destroy_bt_shape(btShape);
+	bulletdelete(btConvex);
 
 	return btQuery.m_count;
 }
@@ -163,7 +163,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()) {
-		shape->destroy_bt_shape(btShape);
+		bulletdelete(btShape);
 		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
 		return false;
 	}
@@ -177,7 +177,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
 	bt_xform_to.getOrigin() += bt_motion;
 
 	if ((bt_xform_to.getOrigin() - bt_xform_from.getOrigin()).fuzzyZero()) {
-		shape->destroy_bt_shape(btShape);
+		bulletdelete(btShape);
 		return false;
 	}
 
@@ -207,7 +207,7 @@ bool BulletPhysicsDirectSpaceState::cast_motion(const RID &p_shape, const Transf
 		r_closest_unsafe = 1.0f;
 	}
 
-	shape->destroy_bt_shape(btShape);
+	bulletdelete(bt_convex_shape);
 	return true; // Mean success
 }
 
@@ -222,7 +222,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()) {
-		shape->destroy_bt_shape(btShape);
+		bulletdelete(btShape);
 		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
 		return false;
 	}
@@ -243,7 +243,7 @@ bool BulletPhysicsDirectSpaceState::collide_shape(RID p_shape, const Transform &
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
 	r_result_count = btQuery.m_count;
-	shape->destroy_bt_shape(btShape);
+	bulletdelete(btConvex);
 
 	return btQuery.m_count;
 }
@@ -254,7 +254,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()) {
-		shape->destroy_bt_shape(btShape);
+		bulletdelete(btShape);
 		ERR_PRINT("The shape is not a convex shape, then is not supported: shape type: " + itos(shape->get_type()));
 		return false;
 	}
@@ -274,7 +274,7 @@ bool BulletPhysicsDirectSpaceState::rest_info(RID p_shape, const Transform &p_sh
 	btQuery.m_closestDistanceThreshold = 0;
 	space->dynamicsWorld->contactTest(&collision_object, btQuery);
 
-	shape->destroy_bt_shape(btShape);
+	bulletdelete(btConvex);
 
 	if (btQuery.m_collided) {
 		if (btCollisionObject::CO_RIGID_BODY == btQuery.m_rest_info_collision_object->getInternalType()) {
@@ -349,46 +349,14 @@ SpaceBullet::~SpaceBullet() {
 	destroy_world();
 }
 
-void SpaceBullet::add_to_pre_flush_queue(CollisionObjectBullet *p_co) {
-	if (p_co->is_in_flush_queue == false) {
-		p_co->is_in_flush_queue = true;
-		queue_pre_flush.push_back(p_co);
-	}
-}
-
-void SpaceBullet::add_to_flush_queue(CollisionObjectBullet *p_co) {
-	if (p_co->is_in_flush_queue == false) {
-		p_co->is_in_flush_queue = true;
-		queue_flush.push_back(p_co);
-	}
-}
-
-void SpaceBullet::remove_from_any_queue(CollisionObjectBullet *p_co) {
-	if (p_co->is_in_flush_queue) {
-		p_co->is_in_flush_queue = false;
-		queue_pre_flush.erase(p_co);
-		queue_flush.erase(p_co);
-	}
-}
-
 void SpaceBullet::flush_queries() {
-	for (uint32_t i = 0; i < queue_pre_flush.size(); i += 1) {
-		queue_pre_flush[i]->dispatch_callbacks();
-		queue_pre_flush[i]->is_in_flush_queue = false;
-	}
-	for (uint32_t i = 0; i < queue_flush.size(); i += 1) {
-		queue_flush[i]->dispatch_callbacks();
-		queue_flush[i]->is_in_flush_queue = false;
+	const btCollisionObjectArray &colObjArray = dynamicsWorld->getCollisionObjectArray();
+	for (int i = colObjArray.size() - 1; 0 <= i; --i) {
+		static_cast<CollisionObjectBullet *>(colObjArray[i]->getUserPointer())->dispatch_callbacks();
 	}
-	queue_pre_flush.clear();
-	queue_flush.clear();
 }
 
 void SpaceBullet::step(real_t p_delta_time) {
-	for (uint32_t i = 0; i < collision_objects.size(); i += 1) {
-		collision_objects[i]->pre_process();
-	}
-
 	delta_time = p_delta_time;
 	dynamicsWorld->stepSimulation(p_delta_time, 0, 0);
 }
@@ -481,30 +449,16 @@ 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) {
-	if (p_area->is_in_world) {
-		areas.erase(p_area);
-		dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
-		p_area->is_in_world = false;
-	}
+	areas.erase(p_area);
+	dynamicsWorld->removeCollisionObject(p_area->get_bt_ghost());
 }
 
 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();
@@ -514,47 +468,24 @@ 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) {
-	remove_from_any_queue(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_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;
+	if (p_body->is_static()) {
+		dynamicsWorld->removeCollisionObject(p_body->get_bt_rigid_body());
+	} else {
+		dynamicsWorld->removeRigidBody(p_body->get_bt_rigid_body());
 	}
 }
 
 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();
@@ -734,7 +665,7 @@ void SpaceBullet::check_ghost_overlaps() {
 
 		/// 1. Reset all states
 		for (i = area->overlappingObjects.size() - 1; 0 <= i; --i) {
-			AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects[i];
+			AreaBullet::OverlappingObjectData &otherObj = area->overlappingObjects.write[i];
 			// This check prevent the overwrite of ENTER state
 			// if this function is called more times before dispatchCallbacks
 			if (otherObj.state != AreaBullet::OVERLAP_STATE_ENTER) {

modules/bullet/space_bullet.h

@@ -31,8 +31,8 @@
 #ifndef SPACE_BULLET_H
 #define SPACE_BULLET_H
 
-#include "core/local_vector.h"
 #include "core/variant.h"
+#include "core/vector.h"
 #include "godot_result_callbacks.h"
 #include "rid_bullet.h"
 #include "servers/physics_server_3d.h"
@@ -110,23 +110,16 @@ class SpaceBullet : public RIDBullet {
 	real_t linear_damp = 0.0;
 	real_t angular_damp = 0.0;
 
-	LocalVector<CollisionObjectBullet *> queue_pre_flush;
-	LocalVector<CollisionObjectBullet *> queue_flush;
-	LocalVector<CollisionObjectBullet *> collision_objects;
-	LocalVector<AreaBullet *> areas;
+	Vector<AreaBullet *> areas;
 
-	LocalVector<Vector3> contactDebug;
-	uint32_t contactDebugCount = 0;
+	Vector<Vector3> contactDebug;
+	int contactDebugCount = 0;
 	real_t delta_time = 0.;
 
 public:
 	SpaceBullet();
 	virtual ~SpaceBullet();
 
-	void add_to_flush_queue(CollisionObjectBullet *p_co);
-	void add_to_pre_flush_queue(CollisionObjectBullet *p_co);
-	void remove_from_any_queue(CollisionObjectBullet *p_co);
-
 	void flush_queries();
 	real_t get_delta_time() { return delta_time; }
 	void step(real_t p_delta_time);
@@ -157,9 +150,6 @@ public:
 	void remove_area(AreaBullet *p_area);
 	void reload_collision_filters(AreaBullet *p_area);
 
-	void register_collision_object(CollisionObjectBullet *p_object);
-	void unregister_collision_object(CollisionObjectBullet *p_object);
-
 	void add_rigid_body(RigidBodyBullet *p_body);
 	void remove_rigid_body(RigidBodyBullet *p_body);
 	void reload_collision_filters(RigidBodyBullet *p_body);
@@ -183,7 +173,7 @@ public:
 	}
 	_FORCE_INLINE_ void add_debug_contact(const Vector3 &p_contact) {
 		if (contactDebugCount < contactDebug.size()) {
-			contactDebug[contactDebugCount++] = p_contact;
+			contactDebug.write[contactDebugCount++] = p_contact;
 		}
 	}
 	_FORCE_INLINE_ Vector<Vector3> get_debug_contacts() { return contactDebug; }