feat: physics world now manages active, added and removed entities better

core/src/physics_world.c

@@ -5,29 +5,52 @@
 #include "rigidbody.h"
 
 static List _world_bodies;
+static List _world_bodies_add_queue;
+static List _world_bodies_remove_queue;
 
 void physics_world_init() {
     _world_bodies = list_from_type(PhysicsEntity);
+    _world_bodies_add_queue = list_from_type(PhysicsEntity);
+    _world_bodies_remove_queue = list_from_type(PhysicsEntity);
 }
 
 void physics_world_clean() {
     list_empty(&_world_bodies);
 }
 
-void physics_world_add_entity(PhysicsEntity entity) {
-    list_add(&_world_bodies, &entity);
-}
-
-void physics_world_remove_entity(PhysicsEntity entity) {
+static
+size_t _internal_physics_world_find_index(PhysicsEntity entity) {
     for(size_t i = 0; i < _world_bodies.len; ++i) {
         PhysicsEntity* found = list_at_as(PhysicsEntity, &_world_bodies, i);
         if(found->data == entity.data) {
-            list_erase(&_world_bodies, i);
-            return;
+            return i;
         }
     }
+    ASSERT_RETURN(0, _world_bodies.len, "Failed to find entity %p in world bodies", entity.data);
+}
+
+static
+void _internal_physics_world_add_remove() {
+    size_t index = 0;
+    list_foreach(PhysicsEntity*, entity, &_world_bodies_remove_queue) {
+        index = _internal_physics_world_find_index(*entity);
+        list_erase(&_world_bodies, index);
+    }
     
-    ASSERT_RETURN(0,, "Physics entity with data at %p is not registered in physics world", entity.data);
+    list_empty(&_world_bodies_remove_queue);
+    
+    list_foreach(PhysicsEntity*, entity, &_world_bodies_add_queue) {
+        list_add(&_world_bodies, entity);
+    }
+    list_empty(&_world_bodies_add_queue);
+}
+
+void physics_world_add_entity(PhysicsEntity entity) {
+    list_add(&_world_bodies_add_queue, &entity);
+}
+
+void physics_world_remove_entity(PhysicsEntity entity) {
+    list_add(&_world_bodies_remove_queue, &entity);
 }
 
 static inline
@@ -84,6 +107,7 @@ void _internal_physics_integrate_forces() {
 }
 
 void physics_world_tick() {
+    _internal_physics_world_add_remove();
     _internal_physics_integrate_forces();
     _internal_physics_narrow_collision();
     _internal_physics_apply();