Fix/simplify single-collider joints;

bjornbytes · May 28, 2024 · c7e97b9 · c7e97b9
1 parent 8e704f1
commit c7e97b9
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 65 deletions.
diff --git a/deps/jolt-physics-sharp b/deps/jolt-physics-sharp
diff --git a/src/api/l_physics.c b/src/api/l_physics.c
@@ -212,7 +212,7 @@ static int l_lovrPhysicsNewTerrainShape(lua_State* L) {
 }
 
 static int l_lovrPhysicsNewWeldJoint(lua_State* L) {
-  Collider* a = luax_totype(L, 1, Collider);
+  Collider* a = luax_checktype(L, 1, Collider);
   Collider* b = luax_totype(L, 2, Collider);
   float anchor[3];
   luax_readvec3(L, 3, anchor, NULL);
@@ -223,7 +223,7 @@ static int l_lovrPhysicsNewWeldJoint(lua_State* L) {
 }
 
 static int l_lovrPhysicsNewBallJoint(lua_State* L) {
-  Collider* a = luax_totype(L, 1, Collider);
+  Collider* a = luax_checktype(L, 1, Collider);
   Collider* b = luax_totype(L, 2, Collider);
   float anchor[3];
   luax_readvec3(L, 3, anchor, NULL);
@@ -234,7 +234,7 @@ static int l_lovrPhysicsNewBallJoint(lua_State* L) {
 }
 
 static int l_lovrPhysicsNewConeJoint(lua_State* L) {
-  Collider* a = luax_totype(L, 1, Collider);
+  Collider* a = luax_checktype(L, 1, Collider);
   Collider* b = luax_totype(L, 2, Collider);
   float anchor[3], axis[3];
   int index = luax_readvec3(L, 3, anchor, NULL);
@@ -246,7 +246,7 @@ static int l_lovrPhysicsNewConeJoint(lua_State* L) {
 }
 
 static int l_lovrPhysicsNewDistanceJoint(lua_State* L) {
-  Collider* a = luax_totype(L, 1, Collider);
+  Collider* a = luax_checktype(L, 1, Collider);
   Collider* b = luax_totype(L, 2, Collider);
   float anchor1[3], anchor2[3];
   int index = luax_readvec3(L, 3, anchor1, NULL);
@@ -258,7 +258,7 @@ static int l_lovrPhysicsNewDistanceJoint(lua_State* L) {
 }
 
 static int l_lovrPhysicsNewHingeJoint(lua_State* L) {
-  Collider* a = luax_totype(L, 1, Collider);
+  Collider* a = luax_checktype(L, 1, Collider);
   Collider* b = luax_totype(L, 2, Collider);
   float anchor[3], axis[3];
   int index = luax_readvec3(L, 3, anchor, NULL);
@@ -270,7 +270,7 @@ static int l_lovrPhysicsNewHingeJoint(lua_State* L) {
 }
 
 static int l_lovrPhysicsNewSliderJoint(lua_State* L) {
-  Collider* a = luax_totype(L, 1, Collider);
+  Collider* a = luax_checktype(L, 1, Collider);
   Collider* b = luax_totype(L, 2, Collider);
   float axis[3];
   luax_readvec3(L, 3, axis, NULL);

diff --git a/src/modules/physics/physics.c b/src/modules/physics/physics.c
@@ -2027,15 +2027,13 @@ static JointNode* lovrJointGetNode(Joint* joint, Collider* collider) {
 void lovrJointInit(Joint* joint, Collider* a, Collider* b) {
   World* world = a->world;
 
-  if (a) {
-    if (a->joints) {
-      joint->a.next = a->joints;
-      lovrJointGetNode(a->joints, a)->prev = joint;
-    }
-
-    a->joints = joint;
+  if (a->joints) {
+    joint->a.next = a->joints;
+    lovrJointGetNode(a->joints, a)->prev = joint;
   }
 
+  a->joints = joint;
+
   if (b) {
     if (b->joints) {
       joint->b.next = b->joints;
@@ -2068,15 +2066,13 @@ void lovrJointDestruct(Joint* joint) {
   JPH_TwoBodyConstraint* constraint = (JPH_TwoBodyConstraint*) joint->constraint;
   Collider* a = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody1(constraint));
   Collider* b = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody2(constraint));
-  World* world = a ? a->world : b->world;
+  World* world = a->world;
   JointNode* node;
 
-  if (a) {
-    node = &joint->a;
-    if (node->next) lovrJointGetNode(node->next, a)->prev = node->prev;
-    if (node->prev) lovrJointGetNode(node->prev, a)->next = node->next;
-    else a->joints = node->next;
-  }
+  node = &joint->a;
+  if (node->next) lovrJointGetNode(node->next, a)->prev = node->prev;
+  if (node->prev) lovrJointGetNode(node->prev, a)->next = node->next;
+  else a->joints = node->next;
 
   if (b) {
     node = &joint->b;
@@ -2161,7 +2157,7 @@ float lovrJointGetForce(Joint* joint) {
   JPH_TwoBodyConstraint* constraint = (JPH_TwoBodyConstraint*) joint->constraint;
   Collider* a = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody1(constraint));
   Collider* b = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody2(constraint));
-  World* world = a ? a->world : b->world;
+  World* world = a->world;
 
   JPH_Vec3 v;
   float force[3], x, y;
@@ -2188,7 +2184,7 @@ float lovrJointGetTorque(Joint* joint) {
   JPH_TwoBodyConstraint* constraint = (JPH_TwoBodyConstraint*) joint->constraint;
   Collider* a = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody1(constraint));
   Collider* b = (Collider*) (uintptr_t) JPH_Body_GetUserData(JPH_TwoBodyConstraint_GetBody2(constraint));
-  World* world = a ? a->world : b->world;
+  World* world = a->world;
 
   JPH_Vec3 v;
   float torque[3], x, y;
@@ -2213,11 +2209,8 @@ float lovrJointGetTorque(Joint* joint) {
 // WeldJoint
 
 WeldJoint* lovrWeldJointCreate(Collider* a, Collider* b, float anchor[3]) {
-  lovrCheck(a || b, "Joint requires at least one Collider");
-  lovrCheck(!a || !b || a->world == b->world, "Joint bodies must exist in same World");
-  JPH_Body* bodyA = a ? a->body : NULL;
-  JPH_Body* bodyB = b ? b->body : NULL;
-  World* world = (a ? a : b)->world;
+  lovrCheck(!b || a->world == b->world, "Joint bodies must exist in same World");
+  JPH_Body* parent = b ? b->body : JPH_Body_GetFixedToWorldBody();
 
   WeldJoint* joint = lovrCalloc(sizeof(WeldJoint));
   joint->ref = 1;
@@ -2226,9 +2219,9 @@ WeldJoint* lovrWeldJointCreate(Collider* a, Collider* b, float anchor[3]) {
   JPH_FixedConstraintSettings* settings = JPH_FixedConstraintSettings_Create();
   JPH_FixedConstraintSettings_SetPoint1(settings, vec3_toJolt(anchor));
   JPH_FixedConstraintSettings_SetPoint2(settings, vec3_toJolt(anchor));
-  joint->constraint = (JPH_Constraint*) JPH_FixedConstraintSettings_CreateConstraint(settings, bodyA, bodyB);
+  joint->constraint = (JPH_Constraint*) JPH_FixedConstraintSettings_CreateConstraint(settings, parent, a->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
-  JPH_PhysicsSystem_AddConstraint(world->system, joint->constraint);
+  JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
   lovrJointInit(joint, a, b);
   lovrRetain(joint);
   return joint;
@@ -2237,11 +2230,8 @@ WeldJoint* lovrWeldJointCreate(Collider* a, Collider* b, float anchor[3]) {
 // BallJoint
 
 BallJoint* lovrBallJointCreate(Collider* a, Collider* b, float anchor[3]) {
-  lovrCheck(a || b, "Joint requires at least one Collider");
-  lovrCheck(!a || !b || a->world == b->world, "Joint bodies must exist in same World");
-  JPH_Body* bodyA = a ? a->body : NULL;
-  JPH_Body* bodyB = b ? b->body : NULL;
-  World* world = (a ? a : b)->world;
+  lovrCheck(!b || a->world == b->world, "Joint bodies must exist in same World");
+  JPH_Body* parent = b ? b->body : JPH_Body_GetFixedToWorldBody();
 
   BallJoint* joint = lovrCalloc(sizeof(BallJoint));
   joint->ref = 1;
@@ -2250,9 +2240,9 @@ BallJoint* lovrBallJointCreate(Collider* a, Collider* b, float anchor[3]) {
   JPH_PointConstraintSettings* settings = JPH_PointConstraintSettings_Create();
   JPH_PointConstraintSettings_SetPoint1(settings, vec3_toJolt(anchor));
   JPH_PointConstraintSettings_SetPoint2(settings, vec3_toJolt(anchor));
-  joint->constraint = (JPH_Constraint*) JPH_PointConstraintSettings_CreateConstraint(settings, bodyA, bodyB);
+  joint->constraint = (JPH_Constraint*) JPH_PointConstraintSettings_CreateConstraint(settings, parent, a->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
-  JPH_PhysicsSystem_AddConstraint(world->system, joint->constraint);
+  JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
   lovrJointInit(joint, a, b);
   lovrRetain(joint);
   return joint;
@@ -2261,11 +2251,8 @@ BallJoint* lovrBallJointCreate(Collider* a, Collider* b, float anchor[3]) {
 // ConeJoint
 
 ConeJoint* lovrConeJointCreate(Collider* a, Collider* b, float anchor[3], float axis[3]) {
-  lovrCheck(a || b, "Joint requires at least one Collider");
-  lovrCheck(!a || !b || a->world == b->world, "Joint bodies must exist in same World");
-  JPH_Body* bodyA = a ? a->body : NULL;
-  JPH_Body* bodyB = b ? b->body : NULL;
-  World* world = (a ? a : b)->world;
+  lovrCheck(!b || a->world == b->world, "Joint bodies must exist in same World");
+  JPH_Body* parent = b ? b->body : JPH_Body_GetFixedToWorldBody();
 
   ConeJoint* joint = lovrCalloc(sizeof(ConeJoint));
   joint->ref = 1;
@@ -2276,8 +2263,8 @@ ConeJoint* lovrConeJointCreate(Collider* a, Collider* b, float anchor[3], float
   JPH_ConeConstraintSettings_SetPoint2(settings, vec3_toJolt(anchor));
   JPH_ConeConstraintSettings_SetTwistAxis1(settings, vec3_toJolt(axis));
   JPH_ConeConstraintSettings_SetTwistAxis2(settings, vec3_toJolt(axis));
-  joint->constraint = (JPH_Constraint*) JPH_ConeConstraintSettings_CreateConstraint(settings, bodyA, bodyB);
-  JPH_PhysicsSystem_AddConstraint((a ? a : b)->world->system, joint->constraint);
+  joint->constraint = (JPH_Constraint*) JPH_ConeConstraintSettings_CreateConstraint(settings, parent, a->body);
+  JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
   lovrJointInit(joint, a, b);
   lovrRetain(joint);
   return joint;
@@ -2314,11 +2301,8 @@ void lovrConeJointSetLimit(ConeJoint* joint, float angle) {
 // DistanceJoint
 
 DistanceJoint* lovrDistanceJointCreate(Collider* a, Collider* b, float anchor1[3], float anchor2[3]) {
-  lovrCheck(a || b, "Joint requires at least one Collider");
-  lovrCheck(!a || !b || a->world == b->world, "Joint bodies must exist in same World");
-  JPH_Body* bodyA = a ? a->body : NULL;
-  JPH_Body* bodyB = b ? b->body : NULL;
-  World* world = (a ? a : b)->world;
+  lovrCheck(!b || a->world == b->world, "Joint bodies must exist in same World");
+  JPH_Body* parent = b ? b->body : JPH_Body_GetFixedToWorldBody();
 
   DistanceJoint* joint = lovrCalloc(sizeof(DistanceJoint));
   joint->ref = 1;
@@ -2327,9 +2311,9 @@ DistanceJoint* lovrDistanceJointCreate(Collider* a, Collider* b, float anchor1[3
   JPH_DistanceConstraintSettings* settings = JPH_DistanceConstraintSettings_Create();
   JPH_DistanceConstraintSettings_SetPoint1(settings, vec3_toJolt(anchor1));
   JPH_DistanceConstraintSettings_SetPoint2(settings, vec3_toJolt(anchor2));
-  joint->constraint = (JPH_Constraint*) JPH_DistanceConstraintSettings_CreateConstraint(settings, bodyA, bodyB);
+  joint->constraint = (JPH_Constraint*) JPH_DistanceConstraintSettings_CreateConstraint(settings, parent, a->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
-  JPH_PhysicsSystem_AddConstraint(world->system, joint->constraint);
+  JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
   lovrJointInit(joint, a, b);
   lovrRetain(joint);
   return joint;
@@ -2363,11 +2347,8 @@ void lovrDistanceJointSetSpring(DistanceJoint* joint, float frequency, float dam
 // HingeJoint
 
 HingeJoint* lovrHingeJointCreate(Collider* a, Collider* b, float anchor[3], float axis[3]) {
-  lovrCheck(a || b, "Joint requires at least one Collider");
-  lovrCheck(!a || !b || a->world == b->world, "Joint bodies must exist in same World");
-  JPH_Body* bodyA = a ? a->body : NULL;
-  JPH_Body* bodyB = b ? b->body : NULL;
-  World* world = (a ? a : b)->world;
+  lovrCheck(!b || a->world == b->world, "Joint bodies must exist in same World");
+  JPH_Body* parent = b ? b->body : JPH_Body_GetFixedToWorldBody();
 
   HingeJoint* joint = lovrCalloc(sizeof(HingeJoint));
   joint->ref = 1;
@@ -2378,9 +2359,9 @@ HingeJoint* lovrHingeJointCreate(Collider* a, Collider* b, float anchor[3], floa
   JPH_HingeConstraintSettings_SetPoint2(settings, vec3_toJolt(anchor));
   JPH_HingeConstraintSettings_SetHingeAxis1(settings, vec3_toJolt(axis));
   JPH_HingeConstraintSettings_SetHingeAxis2(settings, vec3_toJolt(axis));
-  joint->constraint = (JPH_Constraint*) JPH_HingeConstraintSettings_CreateConstraint(settings, bodyA, bodyB);
+  joint->constraint = (JPH_Constraint*) JPH_HingeConstraintSettings_CreateConstraint(settings, parent, a->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
-  JPH_PhysicsSystem_AddConstraint(world->system, joint->constraint);
+  JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
   lovrJointInit(joint, a, b);
   lovrRetain(joint);
   return joint;
@@ -2507,21 +2488,18 @@ void lovrHingeJointSetSpring(HingeJoint* joint, float frequency, float damping)
 // SliderJoint
 
 SliderJoint* lovrSliderJointCreate(Collider* a, Collider* b, float axis[3]) {
-  lovrCheck(a || b, "Joint requires at least one Collider");
-  lovrCheck(!a || !b || a->world == b->world, "Joint bodies must exist in same World");
-  JPH_Body* bodyA = a ? a->body : NULL;
-  JPH_Body* bodyB = b ? b->body : NULL;
-  World* world = (a ? a : b)->world;
+  lovrCheck(!b || a->world == b->world, "Joint bodies must exist in same World");
+  JPH_Body* parent = b ? b->body : JPH_Body_GetFixedToWorldBody();
 
   SliderJoint* joint = lovrCalloc(sizeof(SliderJoint));
   joint->ref = 1;
   joint->type = JOINT_SLIDER;
 
   JPH_SliderConstraintSettings* settings = JPH_SliderConstraintSettings_Create();
   JPH_SliderConstraintSettings_SetSliderAxis(settings, vec3_toJolt(axis));
-  joint->constraint = (JPH_Constraint*) JPH_SliderConstraintSettings_CreateConstraint(settings, bodyA, bodyB);
+  joint->constraint = (JPH_Constraint*) JPH_SliderConstraintSettings_CreateConstraint(settings, parent, a->body);
   JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings);
-  JPH_PhysicsSystem_AddConstraint(world->system, joint->constraint);
+  JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint);
   lovrJointInit(joint, a, b);
   lovrRetain(joint);
   return joint;