From 1d4d403ae5090de3e13b9d2640bcbfcb578d4d06 Mon Sep 17 00:00:00 2001 From: bjorn Date: Mon, 22 Apr 2024 20:31:54 -0700 Subject: [PATCH] WeldJoint; Joint:getForce; Joint:getTorque; --- src/api/l_physics.c | 15 +++++++ src/api/l_physics_joints.c | 39 +++++++++++++++++- src/modules/physics/physics.h | 8 ++++ src/modules/physics/physics_jolt.c | 63 ++++++++++++++++++++++++++++++ src/modules/physics/physics_ode.c | 16 ++++++++ 5 files changed, 140 insertions(+), 1 deletion(-) diff --git a/src/api/l_physics.c b/src/api/l_physics.c index 4c114e02..f017223a 100644 --- a/src/api/l_physics.c +++ b/src/api/l_physics.c @@ -15,6 +15,7 @@ StringEntry lovrShapeType[] = { }; StringEntry lovrJointType[] = { + [JOINT_WELD] = ENTRY("weld"), [JOINT_BALL] = ENTRY("ball"), [JOINT_DISTANCE] = ENTRY("distance"), [JOINT_HINGE] = ENTRY("hinge"), @@ -97,6 +98,17 @@ static int l_lovrPhysicsNewWorld(lua_State* L) { return 1; } +static int l_lovrPhysicsNewWeldJoint(lua_State* L) { + Collider* a = luax_checktype(L, 1, Collider); + Collider* b = luax_checktype(L, 2, Collider); + float anchor[3]; + luax_readvec3(L, 3, anchor, NULL); + WeldJoint* joint = lovrWeldJointCreate(a, b, anchor); + luax_pushtype(L, WeldJoint, joint); + lovrRelease(joint, lovrJointDestroy); + return 1; +} + static int l_lovrPhysicsNewBallJoint(lua_State* L) { Collider* a = luax_checktype(L, 1, Collider); Collider* b = luax_checktype(L, 2, Collider); @@ -201,6 +213,7 @@ static int l_lovrPhysicsNewCompoundShape(lua_State* L) { static const luaL_Reg lovrPhysics[] = { { "newWorld", l_lovrPhysicsNewWorld }, + { "newWeldJoint", l_lovrPhysicsNewWeldJoint }, { "newBallJoint", l_lovrPhysicsNewBallJoint }, { "newBoxShape", l_lovrPhysicsNewBoxShape }, { "newCapsuleShape", l_lovrPhysicsNewCapsuleShape }, @@ -218,6 +231,7 @@ static const luaL_Reg lovrPhysics[] = { extern const luaL_Reg lovrWorld[]; extern const luaL_Reg lovrCollider[]; +extern const luaL_Reg lovrWeldJoint[]; extern const luaL_Reg lovrBallJoint[]; extern const luaL_Reg lovrDistanceJoint[]; extern const luaL_Reg lovrHingeJoint[]; @@ -236,6 +250,7 @@ int luaopen_lovr_physics(lua_State* L) { luax_register(L, lovrPhysics); luax_registertype(L, World); luax_registertype(L, Collider); + luax_registertype(L, WeldJoint); luax_registertype(L, BallJoint); luax_registertype(L, DistanceJoint); luax_registertype(L, HingeJoint); diff --git a/src/api/l_physics_joints.c b/src/api/l_physics_joints.c index 2b7c06a1..9e2be3aa 100644 --- a/src/api/l_physics_joints.c +++ b/src/api/l_physics_joints.c @@ -5,6 +5,7 @@ void luax_pushjoint(lua_State* L, Joint* joint) { switch (lovrJointGetType(joint)) { + case JOINT_WELD: luax_pushtype(L, WeldJoint, joint); break; case JOINT_BALL: luax_pushtype(L, BallJoint, joint); break; case JOINT_DISTANCE: luax_pushtype(L, DistanceJoint, joint); break; case JOINT_HINGE: luax_pushtype(L, HingeJoint, joint); break; @@ -18,6 +19,7 @@ Joint* luax_checkjoint(lua_State* L, int index) { if (p) { const uint64_t hashes[] = { + hash64("WeldJoint", strlen("WeldJoint")), hash64("BallJoint", strlen("BallJoint")), hash64("DistanceJoint", strlen("DistanceJoint")), hash64("HingeJoint", strlen("HingeJoint")), @@ -93,6 +95,20 @@ static int l_lovrJointSetEnabled(lua_State* L) { return 0; } +static int l_lovrJointGetForce(lua_State* L) { + Joint* joint = luax_checkjoint(L, 1); + float force = lovrJointGetForce(joint); + lua_pushnumber(L, force); + return 1; +} + +static int l_lovrJointGetTorque(lua_State* L) { + Joint* joint = luax_checkjoint(L, 1); + float torque = lovrJointGetTorque(joint); + lua_pushnumber(L, torque); + return 1; +} + #define lovrJoint \ { "destroy", l_lovrJointDestroy }, \ { "isDestroyed", l_lovrJointIsDestroyed }, \ @@ -101,7 +117,28 @@ static int l_lovrJointSetEnabled(lua_State* L) { { "getUserData", l_lovrJointGetUserData }, \ { "setUserData", l_lovrJointSetUserData }, \ { "isEnabled", l_lovrJointIsEnabled }, \ - { "setEnabled", l_lovrJointSetEnabled } + { "setEnabled", l_lovrJointSetEnabled }, \ + { "getForce", l_lovrJointGetForce }, \ + { "getTorque", l_lovrJointGetTorque } + +static int l_lovrWeldJointGetAnchors(lua_State* L) { + WeldJoint* joint = luax_checktype(L, 1, WeldJoint); + float anchor1[3], anchor2[3]; + lovrWeldJointGetAnchors(joint, anchor1, anchor2); + lua_pushnumber(L, anchor1[0]); + lua_pushnumber(L, anchor1[1]); + lua_pushnumber(L, anchor1[2]); + lua_pushnumber(L, anchor2[0]); + lua_pushnumber(L, anchor2[1]); + lua_pushnumber(L, anchor2[2]); + return 6; +} + +const luaL_Reg lovrWeldJoint[] = { + lovrJoint, + { "getAnchors", l_lovrWeldJointGetAnchors }, + { NULL, NULL } +}; static int l_lovrBallJointGetAnchors(lua_State* L) { BallJoint* joint = luax_checktype(L, 1, BallJoint); diff --git a/src/modules/physics/physics.h b/src/modules/physics/physics.h index 1d3daed7..318843e7 100644 --- a/src/modules/physics/physics.h +++ b/src/modules/physics/physics.h @@ -22,6 +22,7 @@ typedef Shape MeshShape; typedef Shape TerrainShape; typedef Shape CompoundShape; +typedef Joint WeldJoint; typedef Joint BallJoint; typedef Joint DistanceJoint; typedef Joint HingeJoint; @@ -202,6 +203,7 @@ void lovrCompoundShapeSetChildOffset(CompoundShape* shape, uint32_t index, float // Joints typedef enum { + JOINT_WELD, JOINT_BALL, JOINT_DISTANCE, JOINT_HINGE, @@ -217,6 +219,11 @@ Collider* lovrJointGetColliderB(Joint* joint); Joint* lovrJointGetNext(Joint* joint, Collider* collider); bool lovrJointIsEnabled(Joint* joint); void lovrJointSetEnabled(Joint* joint, bool enable); +float lovrJointGetForce(Joint* joint); +float lovrJointGetTorque(Joint* joint); + +WeldJoint* lovrWeldJointCreate(Collider* a, Collider* b, float anchor[3]); +void lovrWeldJointGetAnchors(WeldJoint* joint, float anchor1[3], float anchor2[3]); BallJoint* lovrBallJointCreate(Collider* a, Collider* b, float anchor[3]); void lovrBallJointGetAnchors(BallJoint* joint, float anchor1[3], float anchor2[3]); @@ -257,6 +264,7 @@ float lovrSliderJointGetUpperLimit(SliderJoint* joint); void lovrSliderJointSetUpperLimit(SliderJoint* joint, float limit); // These tokens need to exist for Lua bindings +#define lovrWeldJointDestroy lovrJointDestroy #define lovrBallJointDestroy lovrJointDestroy #define lovrDistanceJointDestroy lovrJointDestroy #define lovrHingeJointDestroy lovrJointDestroy diff --git a/src/modules/physics/physics_jolt.c b/src/modules/physics/physics_jolt.c index aba19d11..f3ed1048 100644 --- a/src/modules/physics/physics_jolt.c +++ b/src/modules/physics/physics_jolt.c @@ -1156,6 +1156,69 @@ void lovrJointSetEnabled(Joint* joint, bool enable) { JPH_Constraint_SetEnabled(joint->constraint, enable); } +float lovrJointGetForce(Joint* joint) { + JPH_Vec3 v; + float force[3], x, y; + switch (joint->type) { + case JOINT_WELD: + JPH_FixedConstraint_GetTotalLambdaPosition((JPH_FixedConstraint*) joint->constraint, &v); + return vec3_length(vec3_fromJolt(force, &v)); + case JOINT_BALL: + JPH_PointConstraint_GetTotalLambdaPosition((JPH_PointConstraint*) joint->constraint, &v); + return vec3_length(vec3_fromJolt(force, &v)); + case JOINT_DISTANCE: + return JPH_DistanceConstraint_GetTotalLambdaPosition((JPH_DistanceConstraint*) joint->constraint); + case JOINT_HINGE: + JPH_HingeConstraint_GetTotalLambdaPosition((JPH_HingeConstraint*) joint->constraint, &v); + return vec3_length(vec3_fromJolt(force, &v)); + case JOINT_SLIDER: + JPH_SliderConstraint_GetTotalLambdaPosition((JPH_SliderConstraint*) joint->constraint, &x, &y); + return sqrtf((x * x) + (y * y)); + default: return 0.f; + } +} + +float lovrJointGetTorque(Joint* joint) { + JPH_Vec3 v; + float torque[3], x, y; + switch (joint->type) { + case JOINT_WELD: + JPH_FixedConstraint_GetTotalLambdaRotation((JPH_FixedConstraint*) joint->constraint, &v); + return vec3_length(vec3_fromJolt(torque, &v)); + case JOINT_BALL: + return 0.f; + case JOINT_DISTANCE: + return 0.f; + case JOINT_HINGE: + JPH_HingeConstraint_GetTotalLambdaRotation((JPH_HingeConstraint*) joint->constraint, &x, &y); + return sqrtf((x * x) + (y * y)); + case JOINT_SLIDER: + JPH_SliderConstraint_GetTotalLambdaRotation((JPH_SliderConstraint*) joint->constraint, &v); + return vec3_length(vec3_fromJolt(torque, &v)); + default: return 0.f; + } +} + +WeldJoint* lovrWeldJointCreate(Collider* a, Collider* b, float anchor[3]) { + lovrCheck(a->world == b->world, "Joint bodies must exist in same World"); + WeldJoint* joint = lovrCalloc(sizeof(WeldJoint)); + joint->ref = 1; + joint->type = JOINT_WELD; + 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, a->body, b->body); + JPH_ConstraintSettings_Destroy((JPH_ConstraintSettings*) settings); + JPH_PhysicsSystem_AddConstraint(a->world->system, joint->constraint); + lovrJointInit(joint, a, b); + lovrRetain(joint); + return joint; +} + +void lovrWeldJointGetAnchors(WeldJoint* joint, float anchor1[3], float anchor2[3]) { + lovrJointGetAnchors((Joint*) joint, anchor1, anchor2); +} + BallJoint* lovrBallJointCreate(Collider* a, Collider* b, float anchor[3]) { lovrCheck(a->world == b->world, "Joint bodies must exist in same World"); BallJoint* joint = lovrCalloc(sizeof(BallJoint)); diff --git a/src/modules/physics/physics_ode.c b/src/modules/physics/physics_ode.c index 17f0a17b..b3dc1a35 100644 --- a/src/modules/physics/physics_ode.c +++ b/src/modules/physics/physics_ode.c @@ -1102,6 +1102,22 @@ void lovrJointSetEnabled(Joint* joint, bool enable) { } } +float lovrJointGetForce(Joint* joint) { + return 0.f; +} + +float lovrJointGetTorque(Joint* joint) { + return 0.f; +} + +BallJoint* lovrWeldJointCreate(Collider* a, Collider* b, float anchor[3]) { + lovrThrow("NYI"); +} + +void lovrWeldJointGetAnchors(WeldJoint* joint, float anchor1[3], float anchor2[3]) { + // +} + BallJoint* lovrBallJointCreate(Collider* a, Collider* b, float anchor[3]) { lovrCheck(a->world == b->world, "Joint bodies must exist in same World"); BallJoint* joint = lovrCalloc(sizeof(BallJoint));