Rework World Collider/Joint lists;

- Add World:getColliderCount
- Add World:getJointCount
- Add World:getJoints
- Add Joint:isDestroyed
- Destroy joints when either of their colliders are destroyed (Jolt
  seems to require this).
- Fix joint memory leak.
- Change joints to be stored in some crazy graph thing.
  - Reduces allocations, allows constant time insertion/removal.

src/api/l_physics_collider.c

@@ -84,12 +84,13 @@ static int l_lovrColliderSetShapeOffset(lua_State* L) {
 
 static int l_lovrColliderGetJoints(lua_State* L) {
   Collider* collider = luax_checktype(L, 1, Collider);
-  size_t count;
+  lua_newtable(L);
-  Joint** joints = lovrColliderGetJoints(collider, &count);
+  int index = 1;
-  lua_createtable(L, (int) count, 0);
+  void* private;
-  for (size_t i = 0; i < count; i++) {
+  Joint* joint = NULL;
-    luax_pushjoint(L, joints[i]);
+  while ((joint = lovrColliderEnumerateJoints(collider, joint, &private)) != NULL) {
-    lua_rawseti(L, -2, (int) i + 1);
+    luax_pushjoint(L, joint);
+    lua_rawseti(L, -2, index++);
   }
   return 1;
 }

src/api/l_physics_joints.c

@@ -41,6 +41,13 @@ static int l_lovrJointDestroy(lua_State* L) {
   return 0;
 }
 
+static int l_lovrJointIsDestroyed(lua_State* L) {
+  Joint* joint = luax_checkjoint(L, 1);
+  bool destroyed = lovrJointIsDestroyed(joint);
+  lua_pushboolean(L, destroyed);
+  return 1;
+}
+
 static int l_lovrJointGetType(lua_State* L) {
   Joint* joint = luax_checkjoint(L, 1);
   luax_pushenum(L, JointType, lovrJointGetType(joint));
@@ -107,6 +114,7 @@ static int l_lovrJointSetEnabled(lua_State* L) {
 
 #define lovrJoint \
   { "destroy", l_lovrJointDestroy }, \
+  { "isDestroyed", l_lovrJointIsDestroyed }, \
   { "getType", l_lovrJointGetType }, \
   { "getColliders", l_lovrJointGetColliders }, \
   { "getUserData", l_lovrJointGetUserData }, \

src/api/l_physics_world.c

@@ -185,25 +185,10 @@ static int l_lovrWorldNewTerrainCollider(lua_State* L) {
   return 1;
 }
 
-static int l_lovrWorldGetColliders(lua_State* L) {
+static int l_lovrWorldDestroy(lua_State* L) {
   World* world = luax_checktype(L, 1, World);
-
+  lovrWorldDestroyData(world);
-  if (lua_istable(L, 2)) {
+  return 0;
-    lua_settop(L, 2);
-  } else {
-    lua_newtable(L);
-  }
-
-  Collider* collider = lovrWorldGetFirstCollider(world);
-  int index = 1;
-
-  while (collider) {
-    luax_pushtype(L, Collider, collider);
-    lua_rawseti(L, -2, index++);
-    collider = lovrColliderGetNext(collider);
-  }
-
-  return 1;
 }
 
 static int l_lovrWorldGetTags(lua_State* L) {
@@ -219,10 +204,42 @@ static int l_lovrWorldGetTags(lua_State* L) {
   return 1;
 }
 
-static int l_lovrWorldDestroy(lua_State* L) {
+static int l_lovrWorldGetColliders(lua_State* L) {
   World* world = luax_checktype(L, 1, World);
-  lovrWorldDestroyData(world);
+  lua_createtable(L, (int) lovrWorldGetColliderCount(world), 0);
-  return 0;
+  Collider* collider = NULL;
+  int index = 1;
+  while ((collider = lovrWorldEnumerateColliders(world, collider)) != NULL) {
+    luax_pushtype(L, Collider, collider);
+    lua_rawseti(L, -2, index++);
+  }
+  return 1;
+}
+
+static int l_lovrWorldGetColliderCount(lua_State* L) {
+  World* world = luax_checktype(L, 1, World);
+  uint32_t count = lovrWorldGetColliderCount(world);
+  lua_pushinteger(L, count);
+  return 1;
+}
+
+static int l_lovrWorldGetJoints(lua_State* L) {
+  World* world = luax_checktype(L, 1, World);
+  lua_createtable(L, (int) lovrWorldGetJointCount(world), 0);
+  Joint* joint = NULL;
+  int index = 1;
+  while ((joint = lovrWorldEnumerateJoints(world, joint)) != NULL) {
+    luax_pushjoint(L, joint);
+    lua_rawseti(L, -2, index++);
+  }
+  return 1;
+}
+
+static int l_lovrWorldGetJointCount(lua_State* L) {
+  World* world = luax_checktype(L, 1, World);
+  uint32_t count = lovrWorldGetJointCount(world);
+  lua_pushinteger(L, count);
+  return 1;
 }
 
 static int l_lovrWorldUpdate(lua_State* L) {
@@ -514,9 +531,12 @@ const luaL_Reg lovrWorld[] = {
   { "newSphereCollider", l_lovrWorldNewSphereCollider },
   { "newMeshCollider", l_lovrWorldNewMeshCollider },
   { "newTerrainCollider", l_lovrWorldNewTerrainCollider },
-  { "getColliders", l_lovrWorldGetColliders },
-  { "getTags", l_lovrWorldGetTags },
   { "destroy", l_lovrWorldDestroy },
+  { "getTags", l_lovrWorldGetTags },
+  { "getColliderCount", l_lovrWorldGetColliderCount },
+  { "getJointCount", l_lovrWorldGetJointCount },
+  { "getColliders", l_lovrWorldGetColliders },
+  { "getJoints", l_lovrWorldGetJoints },
   { "update", l_lovrWorldUpdate },
   { "computeOverlaps", l_lovrWorldComputeOverlaps },
   { "overlaps", l_lovrWorldOverlaps },

src/modules/physics/physics.h

@@ -54,6 +54,10 @@ typedef struct {
 World* lovrWorldCreate(WorldInfo* info);
 void lovrWorldDestroy(void* ref);
 void lovrWorldDestroyData(World* world);
+uint32_t lovrWorldGetColliderCount(World* world);
+uint32_t lovrWorldGetJointCount(World* world);
+Collider* lovrWorldEnumerateColliders(World* world, Collider* collider);
+Joint* lovrWorldEnumerateJoints(World* world, Joint* joint);
 void lovrWorldUpdate(World* world, float dt, CollisionResolver resolver, void* userdata);
 void lovrWorldComputeOverlaps(World* world);
 int lovrWorldGetNextOverlap(World* world, Shape** a, Shape** b);
@@ -62,7 +66,6 @@ void lovrWorldGetContacts(World* world, Shape* a, Shape* b, Contact contacts[MAX
 void lovrWorldRaycast(World* world, float x1, float y1, float z1, float x2, float y2, float z2, RaycastCallback callback, void* userdata);
 bool lovrWorldQueryBox(World* world, float position[3], float size[3], QueryCallback callback, void* userdata);
 bool lovrWorldQuerySphere(World* world, float position[3], float radius, QueryCallback callback, void* userdata);
-Collider* lovrWorldGetFirstCollider(World* world);
 void lovrWorldGetGravity(World* world, float gravity[3]);
 void lovrWorldSetGravity(World* world, float gravity[3]);
 const char* lovrWorldGetTagName(World* world, uint32_t tag);
@@ -94,12 +97,11 @@ bool lovrColliderIsEnabled(Collider* collider);
 void lovrColliderSetEnabled(Collider* collider, bool enable);
 void lovrColliderInitInertia(Collider* collider, Shape* shape);
 World* lovrColliderGetWorld(Collider* collider);
-Collider* lovrColliderGetNext(Collider* collider);
 Shape* lovrColliderGetShape(Collider* collider, uint32_t child);
 void lovrColliderSetShape(Collider* collider, Shape* shape);
 void lovrColliderGetShapeOffset(Collider* collider, float* position, float* orientation);
 void lovrColliderSetShapeOffset(Collider* collider, float* position, float* orientation);
-Joint** lovrColliderGetJoints(Collider* collider, size_t* count);
+Joint* lovrColliderEnumerateJoints(Collider* collider, Joint* joint, void** private);
 const char* lovrColliderGetTag(Collider* collider);
 bool lovrColliderSetTag(Collider* collider, const char* tag);
 float lovrColliderGetFriction(Collider* collider);
@@ -215,6 +217,7 @@ typedef enum {
 
 void lovrJointDestroy(void* ref);
 void lovrJointDestroyData(Joint* joint);
+bool lovrJointIsDestroyed(Joint* joint);
 JointType lovrJointGetType(Joint* joint);
 float lovrJointGetCFM(Joint* joint);
 void lovrJointSetCFM(Joint* joint, float cfm);

src/modules/physics/physics_jolt.c

@@ -4,16 +4,24 @@
 #include <stdlib.h>
 #include <joltc.h>
 
+typedef struct JointNode {
+  struct JointNode* prev;
+  struct JointNode* next;
+  Joint* joint;
+} JointNode;
+
 struct World {
   uint32_t ref;
   JPH_PhysicsSystem* system;
   JPH_BodyInterface* bodies;
   JPH_ObjectLayerPairFilter* objectLayerPairFilter;
-  int collisionSteps;
+  Collider* colliders;
-  Collider* head;
+  Joint* joints;
+  uint32_t jointCount;
   float defaultLinearDamping;
   float defaultAngularDamping;
   bool defaultIsSleepingAllowed;
+  int collisionSteps;
   char* tags[MAX_TAGS];
 };
 
@@ -25,7 +33,7 @@ struct Collider {
   Shape* shape;
   Collider* prev;
   Collider* next;
-  arr_t(Joint*) joints;
+  JointNode* joints;
   uint32_t tag;
 };
 
@@ -39,6 +47,10 @@ struct Joint {
   uint32_t ref;
   JointType type;
   JPH_Constraint* constraint;
+  JointNode a;
+  JointNode b;
+  Joint* prev;
+  Joint* next;
 };
 
 static struct {
@@ -141,14 +153,31 @@ void lovrWorldDestroy(void* ref) {
 }
 
 void lovrWorldDestroyData(World* world) {
-  while (world->head) {
+  while (world->colliders) {
-    Collider* next = world->head->next;
+    Collider* collider = world->colliders;
-    lovrColliderDestroyData(world->head);
+    Collider* next = collider->next;
-    world->head = next;
+    lovrColliderDestroyData(collider);
+    world->colliders = next;
   }
   JPH_PhysicsSystem_Destroy(world->system);
 }
 
+uint32_t lovrWorldGetColliderCount(World* world) {
+  return JPH_PhysicsSystem_GetNumBodies(world->system);
+}
+
+uint32_t lovrWorldGetJointCount(World* world) {
+  return world->jointCount; // Jolt doesn't expose this
+}
+
+Collider* lovrWorldEnumerateColliders(World* world, Collider* collider) {
+  return collider ? collider->next : world->colliders;
+}
+
+Joint* lovrWorldEnumerateJoints(World* world, Joint* joint) {
+  return joint ? joint->next : world->joints;
+}
+
 void lovrWorldUpdate(World* world, float dt, CollisionResolver resolver, void* userdata) {
   JPH_PhysicsSystem_Step(world->system, dt, world->collisionSteps);
 }
@@ -243,10 +272,6 @@ bool lovrWorldQuerySphere(World* world, float position[3], float radius, QueryCa
   return lovrWorldQueryShape(world, state.querySphere, position, scale, callback, userdata);
 }
 
-Collider* lovrWorldGetFirstCollider(World* world) {
-  return world->head;
-}
-
 void lovrWorldGetGravity(World* world, float gravity[3]) {
   JPH_Vec3 g;
   JPH_PhysicsSystem_GetGravity(world->system, &g);
@@ -345,26 +370,21 @@ Collider* lovrColliderCreate(World* world, Shape* shape, float x, float y, float
   lovrColliderSetAngularDamping(collider, world->defaultAngularDamping, 0.f);
   lovrColliderSetSleepingAllowed(collider, world->defaultIsSleepingAllowed);
 
-  arr_init(&collider->joints);
+  if (world->colliders) {
-
+    collider->next = world->colliders;
-  // Adjust the world's collider list
-  if (!world->head) {
-    world->head = collider;
-  } else {
-    collider->next = world->head;
     collider->next->prev = collider;
-    world->head = collider;
   }
 
+  world->colliders = collider;
+
   lovrRetain(collider->shape);
-  lovrRetain(collider); // The world owns a reference to the collider
+  lovrRetain(collider);
   return collider;
 }
 
 void lovrColliderDestroy(void* ref) {
   Collider* collider = ref;
   lovrColliderDestroyData(collider);
-  arr_free(&collider->joints);
   lovrFree(collider);
 }
 
@@ -375,18 +395,20 @@ void lovrColliderDestroyData(Collider* collider) {
 
   lovrRelease(collider->shape, lovrShapeDestroy);
 
-  size_t count;
+  JointNode* node = collider->joints;
-  Joint** joints = lovrColliderGetJoints(collider, &count);
+  while (node) {
-  for (size_t i = 0; i < count; i++) {
+    JointNode* next = node->next;
-    lovrRelease(joints[i], lovrJointDestroy);
+    lovrJointDestroyData(node->joint);
+    node = next;
   }
 
-  JPH_BodyInterface_RemoveBody(collider->world->bodies, collider->id);
+  World* world = collider->world;
+  JPH_BodyInterface_RemoveBody(world->bodies, collider->id);
   collider->body = NULL;
 
   if (collider->next) collider->next->prev = collider->prev;
   if (collider->prev) collider->prev->next = collider->next;
-  if (collider->world->head == collider) collider->world->head = collider->next;
+  if (world->colliders == collider) world->colliders = collider->next;
   collider->next = collider->prev = NULL;
 
   // If the Collider is destroyed, the world lets go of its reference to this Collider
@@ -492,9 +514,15 @@ void lovrColliderSetShapeOffset(Collider* collider, float* position, float* orie
   JPH_BodyInterface_SetShape(collider->world->bodies, collider->id, shape, updateMass, JPH_Activation_Activate);
 }
 
-Joint** lovrColliderGetJoints(Collider* collider, size_t* count) {
+Joint* lovrColliderEnumerateJoints(Collider* collider, Joint* joint, void** private) {
-  *count = collider->joints.length;
+  if (joint) {
-  return collider->joints.data;
+    JointNode* node = *private;
+    *private = &node->next;
+    return node->next->joint;
+  } else {
+    *private = &collider->joints;
+    return collider->joints->joint;
+  }
 }
 
 const char* lovrColliderGetTag(Collider* collider) {
@@ -1087,7 +1115,7 @@ void lovrCompoundShapeSetChildOffset(CompoundShape* shape, uint32_t index, float
 
 // Joints
 
-void lovrJointGetAnchors(Joint* joint, float anchor1[3], float anchor2[3]) {
+static void lovrJointGetAnchors(Joint* joint, float anchor1[3], float anchor2[3]) {
   JPH_Body* body1 = JPH_TwoBodyConstraint_GetBody1((JPH_TwoBodyConstraint*) joint->constraint);
   JPH_Body* body2 = JPH_TwoBodyConstraint_GetBody2((JPH_TwoBodyConstraint*) joint->constraint);
   JPH_RMatrix4x4 centerOfMassTransform1;
@@ -1108,6 +1136,38 @@ void lovrJointGetAnchors(Joint* joint, float anchor1[3], float anchor2[3]) {
   anchor2[2] = constraintToBody2.m43;
 }
 
+void lovrJointInit(Joint* joint, Collider* a, Collider* b) {
+  JointNode* nodeA = &joint->a;
+  JointNode* nodeB = &joint->b;
+
+  nodeA->joint = joint;
+  nodeB->joint = joint;
+
+  if (a->joints) {
+    nodeA->next = a->joints;
+    a->joints->prev = nodeA;
+  }
+
+  a->joints = nodeA;
+
+  if (b->joints) {
+    nodeB->next = b->joints;
+    b->joints->prev = nodeB;
+  }
+
+  b->joints = nodeB;
+
+  World* world = a->world;
+
+  if (world->joints) {
+    joint->next = world->joints;
+    joint->next->prev = joint;
+  }
+
+  world->joints = joint;
+  world->jointCount++;
+}
+
 void lovrJointDestroy(void* ref) {
   Joint* joint = ref;
   lovrJointDestroyData(joint);
@@ -1119,36 +1179,38 @@ void lovrJointDestroyData(Joint* joint) {
     return;
   }
 
-  JPH_PhysicsSystem* system;
+  JPH_TwoBodyConstraint* constraint = (JPH_TwoBodyConstraint*) joint->constraint;
-  JPH_Body* bodyA = JPH_TwoBodyConstraint_GetBody1((JPH_TwoBodyConstraint*) joint->constraint);
+  Collider* a = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody1(constraint));
-  if (bodyA) {
+  Collider* b = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody2(constraint));
-    Collider* collider = (Collider*) JPH_Body_GetUserData(bodyA);
-    system = collider->world->system;
-    for (size_t i = 0; i < collider->joints.length; i++) {
-      if (collider->joints.data[i] == joint) {
-        arr_splice(&collider->joints, i, 1);
-        break;
-      }
-    }
-  }
 
-  JPH_Body* bodyB = JPH_TwoBodyConstraint_GetBody2((JPH_TwoBodyConstraint*) joint->constraint);
+  JointNode* nodeA = &joint->a;
-  if (bodyB) {
+  if (nodeA->next) nodeA->next->prev = nodeA->prev;
-    Collider* collider = (Collider*) JPH_Body_GetUserData(bodyB);
+  if (nodeA->prev) nodeA->prev->next = nodeA->next;
-    for (size_t i = 0; i < collider->joints.length; i++) {
+  if (nodeA == a->joints) a->joints = nodeA->next;
-      if (collider->joints.data[i] == joint) {
+
-        arr_splice(&collider->joints, i, 1);
+  JointNode* nodeB = &joint->b;
-        break;
+  if (nodeB->next) nodeB->next->prev = nodeB->prev;
-      }
+  if (nodeB->prev) nodeB->prev->next = nodeB->next;
-    }
+  if (nodeB == b->joints) b->joints = nodeB->next;
-  }
+
-  if (system) {
+  World* world = a->world;
-    JPH_PhysicsSystem_RemoveConstraint(system, joint->constraint);
+  if (joint->next) joint->next->prev = joint->prev;
-  }
+  if (joint->prev) joint->prev->next = joint->next;
+  if (world->joints == joint) world->joints = joint->next;
+  joint->next = joint->prev = NULL;
+  world->jointCount--;
+
+  JPH_PhysicsSystem_RemoveConstraint(world->system, joint->constraint);
+  JPH_Constraint_Destroy(joint->constraint);
   joint->constraint = NULL;
+
   lovrRelease(joint, lovrJointDestroy);
 }
 
+bool lovrJointIsDestroyed(Joint* joint) {
+  return !joint->constraint;
+}
+
 JointType lovrJointGetType(Joint* joint) {
   return joint->type;
 }
@@ -1188,8 +1250,7 @@ BallJoint* lovrBallJointCreate(Collider* a, Collider* b, float anchor[3]) {
   joint->constraint = (JPH_Constraint*) JPH_PointConstraintSettings_CreateConstraint(settings, a->body, b->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
   JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
-  arr_push(&a->joints, joint);
+  lovrJointInit(joint, a, b);
-  arr_push(&b->joints, joint);
   lovrRetain(joint);
   return joint;
 }
@@ -1237,8 +1298,7 @@ DistanceJoint* lovrDistanceJointCreate(Collider* a, Collider* b, float anchor1[3
   joint->constraint = (JPH_Constraint*) JPH_DistanceConstraintSettings_CreateConstraint(settings, a->body, b->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
   JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
-  arr_push(&a->joints, joint);
+  lovrJointInit(joint, a, b);
-  arr_push(&b->joints, joint);
   lovrRetain(joint);
   return joint;
 }
@@ -1300,8 +1360,7 @@ HingeJoint* lovrHingeJointCreate(Collider* a, Collider* b, float anchor[3], floa
   joint->constraint = (JPH_Constraint*) JPH_HingeConstraintSettings_CreateConstraint(settings, a->body, b->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
   JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
-  arr_push(&a->joints, joint);
+  lovrJointInit(joint, a, b);
-  arr_push(&b->joints, joint);
   lovrRetain(joint);
   return joint;
 }
@@ -1377,8 +1436,7 @@ SliderJoint* lovrSliderJointCreate(Collider* a, Collider* b, float axis[3]) {
   joint->constraint = (JPH_Constraint*) JPH_SliderConstraintSettings_CreateConstraint(settings, a->body, b->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
   JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
-  arr_push(&a->joints, joint);
+  lovrJointInit(joint, a, b);
-  arr_push(&b->joints, joint);
   lovrRetain(joint);
   return joint;
 }

src/modules/physics/physics_ode.c

@@ -206,6 +206,28 @@ void lovrWorldDestroyData(World* world) {
   }
 }
 
+uint32_t lovrWorldGetColliderCount(World* world) {
+  Collider* collider = world->head;
+  uint32_t count = 0;
+  while (collider) {
+    collider = collider->next;
+    count++;
+  }
+  return count;
+}
+
+uint32_t lovrWorldGetJointCount(World* world) {
+  return 0;
+}
+
+Collider* lovrWorldEnumerateColliders(World* world, Collider* collider) {
+  return collider ? collider->next : world->head;
+}
+
+Joint* lovrWorldEnumerateJoints(World* world, Joint* joint) {
+  return NULL;
+}
+
 void lovrWorldUpdate(World* world, float dt, CollisionResolver resolver, void* userdata) {
   if (resolver) {
     resolver(world, userdata);
@@ -335,10 +357,6 @@ bool lovrWorldQuerySphere(World* world, float position[3], float radius, QueryCa
   return data.called;
 }
 
-Collider* lovrWorldGetFirstCollider(World* world) {
-  return world->head;
-}
-
 void lovrWorldGetGravity(World* world, float gravity[3]) {
   dReal g[4];
   dWorldGetGravity(world->id, g);
@@ -478,8 +496,8 @@ void lovrColliderDestroyData(Collider* collider) {
 
   lovrColliderSetShape(collider, NULL);
 
-  size_t count;
+  Joint** joints = collider->joints.data;
-  Joint** joints = lovrColliderGetJoints(collider, &count);
+  size_t count = collider->joints.length;
   for (size_t i = 0; i < count; i++) {
     lovrRelease(joints[i], lovrJointDestroy);
   }
@@ -583,6 +601,15 @@ Joint** lovrColliderGetJoints(Collider* collider, size_t* count) {
   return collider->joints.data;
 }
 
+Joint* lovrColliderEnumerateJoints(Collider* collider, Joint* joint, void** private) {
+  if (!joint) {
+    *private = NULL;
+  }
+
+  uintptr_t index = (uintptr_t) *private;
+  return index < collider->joints.length ? collider->joints.data[index] : NULL;
+}
+
 const char* lovrColliderGetTag(Collider* collider) {
   return lovrWorldGetTagName(collider->world, collider->tag);
 }
@@ -1160,6 +1187,10 @@ void lovrJointDestroyData(Joint* joint) {
   }
 }
 
+bool lovrJointIsDestroyed(Joint* joint) {
+  return joint->id == NULL;
+}
+
 JointType lovrJointGetType(Joint* joint) {
   return joint->type;
 }