Merge pull request #29951 from raphael10241024/add_physics_2d_optimize

add 2d physic optimization similar to 3d physics

6 files changed, 71 insertions, 13 deletions

diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp
index b1c21290ab..39de440da2 100644
--- a/servers/physics/collision_object_sw.cpp
+++ b/servers/physics/collision_object_sw.cpp
@@ -76,6 +76,13 @@ void CollisionObjectSW::set_shape_transform(int p_index, const Transform &p_tran
 	//_shapes_changed();
 }
 
+void CollisionObjectSW::set_shape_as_disabled(int p_idx, bool p_enable) {
+	shapes.write[p_idx].disabled = p_enable;
+	if (!pending_shape_update_list.in_list()) {
+		PhysicsServerSW::singleton->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+}
+
 void CollisionObjectSW::remove_shape(ShapeSW *p_shape) {
 
 	//remove a shape, all the times it appears
diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h
index c2c3fe0e5a..895eda8528 100644
--- a/servers/physics/collision_object_sw.h
+++ b/servers/physics/collision_object_sw.h
@@ -139,8 +139,11 @@ public:
 	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
 
-	_FORCE_INLINE_ void set_shape_as_disabled(int p_idx, bool p_enable) { shapes.write[p_idx].disabled = p_enable; }
-	_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const { return shapes[p_idx].disabled; }
+	void set_shape_as_disabled(int p_idx, bool p_enable);
+	_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const {
+		CRASH_BAD_INDEX(p_idx, shapes.size());
+		return shapes[p_idx].disabled;
+	}
 
 	_FORCE_INLINE_ void set_collision_layer(uint32_t p_layer) { collision_layer = p_layer; }
 	_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 445a2e0613..f556638db1 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -29,6 +29,7 @@
 /*************************************************************************/
 
 #include "collision_object_2d_sw.h"
+#include "servers/physics_2d/physics_2d_server_sw.h"
 #include "space_2d_sw.h"
 
 void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_transform, bool p_disabled) {
@@ -43,8 +44,12 @@ void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_tra
 	s.one_way_collision_margin = 0;
 	shapes.push_back(s);
 	p_shape->add_owner(this);
-	_update_shapes();
-	_shapes_changed();
+
+	if (!pending_shape_update_list.in_list()) {
+		Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+	// _update_shapes();
+	// _shapes_changed();
 }
 
 void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
@@ -54,8 +59,12 @@ void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
 	shapes.write[p_index].shape = p_shape;
 
 	p_shape->add_owner(this);
-	_update_shapes();
-	_shapes_changed();
+
+	if (!pending_shape_update_list.in_list()) {
+		Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+	// _update_shapes();
+	// _shapes_changed();
 }
 
 void CollisionObject2DSW::set_shape_metadata(int p_index, const Variant &p_metadata) {
@@ -70,8 +79,12 @@ void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_
 
 	shapes.write[p_index].xform = p_transform;
 	shapes.write[p_index].xform_inv = p_transform.affine_inverse();
-	_update_shapes();
-	_shapes_changed();
+
+	if (!pending_shape_update_list.in_list()) {
+		Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+	// _update_shapes();
+	// _shapes_changed();
 }
 
 void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
@@ -89,9 +102,15 @@ void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
 	if (p_disabled && shape.bpid != 0) {
 		space->get_broadphase()->remove(shape.bpid);
 		shape.bpid = 0;
-		_update_shapes();
+		if (!pending_shape_update_list.in_list()) {
+			Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		}
+		//_update_shapes();
 	} else if (!p_disabled && shape.bpid == 0) {
-		_update_shapes(); // automatically adds shape with bpid == 0
+		if (!pending_shape_update_list.in_list()) {
+			Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		}
+		//_update_shapes(); // automatically adds shape with bpid == 0
 	}
 }
 
@@ -122,8 +141,11 @@ void CollisionObject2DSW::remove_shape(int p_index) {
 	shapes[p_index].shape->remove_owner(this);
 	shapes.remove(p_index);
 
-	_update_shapes();
-	_shapes_changed();
+	if (!pending_shape_update_list.in_list()) {
+		Physics2DServerSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+	}
+	// _update_shapes();
+	// _shapes_changed();
 }
 
 void CollisionObject2DSW::_set_static(bool p_static) {
@@ -239,7 +261,8 @@ void CollisionObject2DSW::_shape_changed() {
 	_shapes_changed();
 }
 
-CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
+CollisionObject2DSW::CollisionObject2DSW(Type p_type) :
+		pending_shape_update_list(this) {
 
 	_static = true;
 	type = p_type;
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index fa18e61262..ed59469878 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -78,6 +78,8 @@ private:
 	uint32_t collision_layer;
 	bool _static;
 
+	SelfList<CollisionObject2DSW> pending_shape_update_list;
+
 	void _update_shapes();
 
 protected:
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 0efa15d43e..cc656d3b73 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -902,6 +902,8 @@ void Physics2DServerSW::body_apply_torque_impulse(RID p_body, real_t p_torque) {
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
+	_update_shapes();
+
 	body->apply_torque_impulse(p_torque);
 }
 
@@ -910,6 +912,8 @@ void Physics2DServerSW::body_apply_impulse(RID p_body, const Vector2 &p_pos, con
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
+	_update_shapes();
+
 	body->apply_impulse(p_pos, p_impulse);
 	body->wakeup();
 };
@@ -944,6 +948,8 @@ void Physics2DServerSW::body_set_axis_velocity(RID p_body, const Vector2 &p_axis
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
+	_update_shapes();
+
 	Vector2 v = body->get_linear_velocity();
 	Vector2 axis = p_axis_velocity.normalized();
 	v -= axis * axis.dot(v);
@@ -1052,6 +1058,8 @@ bool Physics2DServerSW::body_test_motion(RID p_body, const Transform2D &p_from,
 	ERR_FAIL_COND_V(!body->get_space(), false);
 	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
 
+	_update_shapes();
+
 	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, p_margin, r_result, p_exclude_raycast_shapes);
 }
 
@@ -1238,6 +1246,8 @@ Physics2DServer::JointType Physics2DServerSW::joint_get_type(RID p_joint) const
 
 void Physics2DServerSW::free(RID p_rid) {
 
+	_update_shapes(); // just in case
+
 	if (shape_owner.owns(p_rid)) {
 
 		Shape2DSW *shape = shape_owner.get(p_rid);
@@ -1335,6 +1345,8 @@ void Physics2DServerSW::step(real_t p_step) {
 	if (!active)
 		return;
 
+	_update_shapes();
+
 	doing_sync = false;
 
 	last_step = p_step;
@@ -1418,6 +1430,14 @@ void Physics2DServerSW::finish() {
 	memdelete(direct_state);
 };
 
+void Physics2DServerSW::_update_shapes() {
+
+	while (pending_shape_update_list.first()) {
+		pending_shape_update_list.first()->self()->_shape_changed();
+		pending_shape_update_list.remove(pending_shape_update_list.first());
+	}
+}
+
 int Physics2DServerSW::get_process_info(ProcessInfo p_info) {
 
 	switch (p_info) {
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index adc011af40..72625c397c 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -70,6 +70,9 @@ class Physics2DServerSW : public Physics2DServer {
 	static Physics2DServerSW *singletonsw;
 
 	//void _clear_query(Query2DSW *p_query);
+	friend class CollisionObject2DSW;
+	SelfList<CollisionObject2DSW>::List pending_shape_update_list;
+	void _update_shapes();
 
 	RID _shape_create(ShapeType p_shape);