From 7303770df3f6c09741b5e463b83debc2b809b139 Mon Sep 17 00:00:00 2001 From: Foxyz Date: Fri, 2 Oct 2026 17:34:54 +0300 Subject: [PATCH] Angular limits for drive-mode Generic6DOFJoint3D (shoulder range of motion) Box3D's b3MotorJoint has no angular limits, so a drive used as a shoulder could be bent into the chest or behind the back. patches/box3d/0001 adds them to the motor joint (applied to the box3d checkout at configure time, submodule stays upstream): - swing limit: per-quadrant ellipse on the swing rotation vector (asymmetric lower/upper bounds about the limit frame's x and y), with a Jacobian that includes the radius change along the boundary; - twist limit about frame B's z, exact twist Jacobian; - own limit frames, so moving the drive target (frame A) never moves the range; - warm starting, joint torque/impulse reporting, recording writer/reader. The wrapper enables them when any angular axis of a DRIVE has a real range (limit flag on, lower < upper): X = twist, Y/Z = swing bounds, measured between the joint frames as created. Limit constraints run at 120 Hz softness. Co-Authored-By: Claude Opus 5.5 --- CMakeLists.txt | 15 + .../0001-motor-joint-angular-limits.patch | 408 ++++++++++++++++++ .../box3d_generic_6dof_joint_impl_3d.cpp | 46 ++ .../box3d_generic_6dof_joint_impl_3d.hpp | 7 + 4 files changed, 476 insertions(+) create mode 100644 patches/box3d/0001-motor-joint-angular-limits.patch diff --git a/CMakeLists.txt b/CMakeLists.txt index 06ef797..d0b87a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,21 @@ set(BOX3D_BENCHMARKS OFF CACHE BOOL "" FORCE) set(BOX3D_DOCS OFF CACHE BOOL "" FORCE) set(BOX3D_UNIT_TESTS OFF CACHE BOOL "" FORCE) +# Local additions to Box3D itself live as patches (the submodule stays pinned to upstream). +# Applied to the checkout at configure time; a marker string per patch makes this idempotent. +# 0001 — angular (swing ellipse + twist) limits on b3MotorJoint (marker b3MotorJointSwingLimit) +find_package(Git REQUIRED) +file(READ "${CMAKE_CURRENT_SOURCE_DIR}/box3d/src/motor_joint.c" GODOT_BOX3D_MOTOR_JOINT_SOURCE) +if(NOT GODOT_BOX3D_MOTOR_JOINT_SOURCE MATCHES "b3MotorJointSwingLimit") + execute_process( + COMMAND "${GIT_EXECUTABLE}" -C "${CMAKE_CURRENT_SOURCE_DIR}/box3d" apply + "${CMAKE_CURRENT_SOURCE_DIR}/patches/box3d/0001-motor-joint-angular-limits.patch" + RESULT_VARIABLE GODOT_BOX3D_PATCH_RESULT) + if(NOT GODOT_BOX3D_PATCH_RESULT EQUAL 0) + message(FATAL_ERROR "Failed to apply patches/box3d/0001-motor-joint-angular-limits.patch to box3d/") + endif() +endif() + add_subdirectory(box3d) # --- godot-cpp (fetched via ExternalProject_Add) --- diff --git a/patches/box3d/0001-motor-joint-angular-limits.patch b/patches/box3d/0001-motor-joint-angular-limits.patch new file mode 100644 index 0000000..3208bae --- /dev/null +++ b/patches/box3d/0001-motor-joint-angular-limits.patch @@ -0,0 +1,408 @@ +diff -ruN a/include/box3d/types.h b/include/box3d/types.h +--- a/include/box3d/types.h 2026-10-01 17:24:25.919048141 +0300 ++++ b/include/box3d/types.h 2026-10-02 17:18:19.916301593 +0300 +@@ -740,6 +740,28 @@ + + /// Maximum spring torque in newton-meters + float maxSpringTorque; ++ ++ /// Angular limit frames (rotation only, relative to each body's origin). The limits measure the ++ /// rotation of limit frame B relative to limit frame A: the swing of the z-axis away from frame A's ++ /// z-axis and the twist about it. Independent of localFrameA/B, so moving the drive target (frame A) ++ /// does not move the limits. Default identity. ++ b3Quat limitRotationA; ++ b3Quat limitRotationB; ++ ++ /// Swing limit. The swing is the rotation vector taking limit frame A's z-axis onto limit frame B's ++ /// z-axis; its components about frame A's x and y axes must stay inside the ellipse spanned by these ++ /// per-quadrant bounds (radians, lower <= 0 <= upper, magnitudes up to 0.99 pi). Equal bounds give a ++ /// circular cone; unequal ones an asymmetric (e.g. anatomical) range. ++ bool enableSwingLimit; ++ float lowerSwingX; ++ float upperSwingX; ++ float lowerSwingY; ++ float upperSwingY; ++ ++ /// Twist limit about limit frame B's z-axis, radians in [-0.99 pi, 0.99 pi]. ++ bool enableTwistLimit; ++ float lowerTwistAngle; ++ float upperTwistAngle; + } b3MotorJointDef; + + /// Use this to initialize your joint definition +diff -ruN a/src/joint.c b/src/joint.c +--- a/src/joint.c 2026-10-01 17:24:25.937994385 +0300 ++++ b/src/joint.c 2026-10-02 17:18:19.921160668 +0300 +@@ -59,6 +59,8 @@ + { + b3MotorJointDef def = { 0 }; + def.base = b3DefaultJointDef(); ++ def.limitRotationA = b3Quat_identity; ++ def.limitRotationB = b3Quat_identity; + return def; + } + +@@ -495,6 +497,18 @@ + joint->motorJoint.angularHertz = def->angularHertz; + joint->motorJoint.angularDampingRatio = def->angularDampingRatio; + joint->motorJoint.maxSpringTorque = def->maxSpringTorque; ++ joint->motorJoint.limitRotationA = b3NormalizeQuat( def->limitRotationA ); ++ joint->motorJoint.limitRotationB = b3NormalizeQuat( def->limitRotationB ); ++ joint->motorJoint.enableSwingLimit = def->enableSwingLimit; ++ joint->motorJoint.lowerSwingX = b3ClampFloat( def->lowerSwingX, -0.99f * B3_PI, 0.0f ); ++ joint->motorJoint.upperSwingX = b3ClampFloat( def->upperSwingX, 0.0f, 0.99f * B3_PI ); ++ joint->motorJoint.lowerSwingY = b3ClampFloat( def->lowerSwingY, -0.99f * B3_PI, 0.0f ); ++ joint->motorJoint.upperSwingY = b3ClampFloat( def->upperSwingY, 0.0f, 0.99f * B3_PI ); ++ joint->motorJoint.enableTwistLimit = def->enableTwistLimit; ++ joint->motorJoint.lowerTwistAngle = ++ b3ClampFloat( b3MinFloat( def->lowerTwistAngle, def->upperTwistAngle ), -0.99f * B3_PI, 0.99f * B3_PI ); ++ joint->motorJoint.upperTwistAngle = ++ b3ClampFloat( b3MaxFloat( def->lowerTwistAngle, def->upperTwistAngle ), -0.99f * B3_PI, 0.99f * B3_PI ); + + b3JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + B3_REC_CREATE( world, CreateMotorJoint, jointId, worldId, *def ); +@@ -1028,7 +1042,10 @@ + { + b3MotorJoint* joint = &sim->motorJoint; + linearImpulse = b3Length( b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ) ); +- angularImpulse = b3Length( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ) ); ++ b3Vec3 limitImpulse = b3MulSV( -joint->swingImpulse, joint->swingAxis ); ++ limitImpulse = b3MulAdd( limitImpulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, joint->twistJacobian ); ++ angularImpulse = ++ b3Length( b3Add( b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ), limitImpulse ) ); + } + break; + +@@ -1570,7 +1587,7 @@ + break; + + case b3_motorJoint: +- b3SolveMotorJoint( joint, context ); ++ b3SolveMotorJoint( joint, context, useBias ); + break; + + case b3_prismaticJoint: +diff -ruN a/src/joint.h b/src/joint.h +--- a/src/joint.h 2026-10-01 17:24:25.937994385 +0300 ++++ b/src/joint.h 2026-10-02 17:25:15.094307396 +0300 +@@ -119,6 +119,22 @@ + b3Transform frameB; + b3Vec3 deltaCenter; + b3Matrix3 angularMass; ++ ++ // angular limits (see b3MotorJointDef) ++ b3Quat limitRotationA; ++ b3Quat limitRotationB; ++ bool enableSwingLimit; ++ bool enableTwistLimit; ++ float lowerSwingX, upperSwingX, lowerSwingY, upperSwingY; ++ float lowerTwistAngle, upperTwistAngle; ++ ++ b3Quat limitFrameA; ++ b3Quat limitFrameB; ++ b3Vec3 swingAxis; // last solved swing Jacobian (negated), for warm starting ++ b3Vec3 twistJacobian; // last solved twist Jacobian, for warm starting ++ float swingImpulse; ++ float lowerTwistImpulse; ++ float upperTwistImpulse; + } b3MotorJoint; + + typedef struct b3ParallelJoint +@@ -398,7 +414,7 @@ + void b3WarmStartWheelJoint( b3JointSim* base, b3StepContext* context ); + + void b3SolveDistanceJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +-void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ); ++void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context, bool useBias ); + void b3SolveParallelJoint( b3JointSim* base, b3StepContext* context ); + void b3SolvePrismaticJoint( b3JointSim* base, b3StepContext* context, bool useBias ); + void b3SolveRevoluteJoint( b3JointSim* base, b3StepContext* context, bool useBias ); +diff -ruN a/src/motor_joint.c b/src/motor_joint.c +--- a/src/motor_joint.c 2026-10-01 17:24:25.937994385 +0300 ++++ b/src/motor_joint.c 2026-10-02 17:26:50.138061833 +0300 +@@ -161,7 +161,25 @@ + + b3Vec3 b3GetMotorJointTorque( b3World* world, b3JointSim* base ) + { +- return b3MulSV( world->inv_h, b3Add( base->motorJoint.angularVelocityImpulse, base->motorJoint.angularSpringImpulse ) ); ++ b3MotorJoint* joint = &base->motorJoint; ++ b3Vec3 impulse = b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ); ++ impulse = b3MulSub( impulse, joint->swingImpulse, joint->swingAxis ); ++ impulse = b3MulAdd( impulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, joint->twistJacobian ); ++ return b3MulSV( world->inv_h, impulse ); ++} ++ ++// Radius of the swing limit ellipse in the direction of the swing rotation axis (x, y), limit frame A. ++// Per-quadrant semi-axes, so lower and upper bounds may differ (anatomical ranges). ++static float b3MotorJointSwingLimit( const b3MotorJoint* joint, float axisX, float axisY ) ++{ ++ float a = axisX >= 0.0f ? joint->upperSwingX : -joint->lowerSwingX; ++ float b = axisY >= 0.0f ? joint->upperSwingY : -joint->lowerSwingY; ++ a = b3MaxFloat( a, 1.0e-3f ); ++ b = b3MaxFloat( b, 1.0e-3f ); ++ float u = axisX / a; ++ float v = axisY / b; ++ float d = u * u + v * v; ++ return d > 0.0f ? b3MinFloat( 1.0f / sqrtf( d ), 0.99f * B3_PI ) : b3MinFloat( b3MinFloat( a, b ), 0.99f * B3_PI ); + } + + // Point-to-point constraint +@@ -228,6 +246,29 @@ + + joint->angularMass = b3InvertMatrix( invInertiaSum ); + ++ // Angular limits: the spherical joint's cone/twist formulation, measured in their own frames so the ++ // drive target (frame A) can move freely. The Jacobians are built in the solve from the current ++ // sub-step rotation; swingAxis/twistJacobian keep the last ones for warm starting. ++ joint->limitFrameA = b3MulQuat( bodySimA->transform.q, joint->limitRotationA ); ++ joint->limitFrameB = b3MulQuat( bodySimB->transform.q, joint->limitRotationB ); ++ if ( base->fixedRotation ) ++ { ++ joint->swingImpulse = 0.0f; ++ joint->lowerTwistImpulse = 0.0f; ++ joint->upperTwistImpulse = 0.0f; ++ } ++ ++ if ( context->enableWarmStarting == false || joint->enableSwingLimit == false ) ++ { ++ joint->swingImpulse = 0.0f; ++ } ++ ++ if ( context->enableWarmStarting == false || joint->enableTwistLimit == false ) ++ { ++ joint->lowerTwistImpulse = 0.0f; ++ joint->upperTwistImpulse = 0.0f; ++ } ++ + if ( context->enableWarmStarting == false ) + { + joint->linearVelocityImpulse = b3Vec3_zero; +@@ -259,6 +300,8 @@ + + b3Vec3 linearImpulse = b3Add( joint->linearVelocityImpulse, joint->linearSpringImpulse ); + b3Vec3 angularImpulse = b3Add( joint->angularVelocityImpulse, joint->angularSpringImpulse ); ++ angularImpulse = b3MulSub( angularImpulse, joint->swingImpulse, joint->swingAxis ); ++ angularImpulse = b3MulAdd( angularImpulse, joint->lowerTwistImpulse - joint->upperTwistImpulse, joint->twistJacobian ); + + stateA->linearVelocity = b3MulSub( stateA->linearVelocity, mA, linearImpulse ); + stateA->angularVelocity = b3Sub( stateA->angularVelocity, b3MulMV( iA, b3Add( b3Cross( rA, linearImpulse ), angularImpulse ) ) ); +@@ -266,7 +309,7 @@ + stateB->angularVelocity = b3Add( stateB->angularVelocity, b3MulMV( iB, b3Add( b3Cross( rB, linearImpulse ), angularImpulse ) ) ); + } + +-void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context ) ++void b3SolveMotorJoint( b3JointSim* base, b3StepContext* context, bool useBias ) + { + B3_ASSERT( base->type == b3_motorJoint ); + +@@ -344,6 +387,161 @@ + wB = b3Add( wB, b3MulMV( iB, impulse ) ); + } + ++ if ( ( joint->enableSwingLimit || joint->enableTwistLimit ) && base->fixedRotation == false ) ++ { ++ // Jacobians are rebuilt from the current sub-step rotation (not the prepare-time axes): with an ++ // asymmetric range the limit radius changes with the swing direction, and a fast arm sliding ++ // along the boundary would otherwise sink deep into the corners. ++ b3Quat limitA = b3MulQuat( stateA->deltaRotation, joint->limitFrameA ); ++ b3Quat limitB = b3MulQuat( stateB->deltaRotation, joint->limitFrameB ); ++ b3Quat limitQ = b3InvMulQuat( limitA, limitB ); ++ b3Matrix3 invInertiaSum = b3AddMM( iA, iB ); ++ ++ // Swing geometry in limit frame A: z = cone axis, zB = frame B's z, n = swing axis (z x zB), ++ // m = in-plane direction of zB (n = z x m). ++ b3Vec3 zB = b3RotateVector( limitQ, b3Vec3_axisZ ); ++ float planar = sqrtf( zB.x * zB.x + zB.y * zB.y ); ++ float swingAngle = b3GetSwingAngle( limitQ ); ++ b3Vec3 n = b3Vec3_zero; ++ b3Vec3 m = b3Vec3_zero; ++ if ( planar > 1.0e-6f ) ++ { ++ m = (b3Vec3){ zB.x / planar, zB.y / planar, 0.0f }; ++ n = (b3Vec3){ -m.y, m.x, 0.0f }; ++ } ++ ++ if ( joint->enableSwingLimit && planar > 1.0e-6f ) ++ { ++ // C = r(phi) - theta >= 0, theta = swing angle, phi = direction of the swing axis n, r = the ++ // per-quadrant ellipse radius. With the relative angular velocity w (frame A): ++ // dtheta/dt = w.n dphi/dt = w.z - cot(theta) w.m ++ // so dC/dt = J.w with J = -n + r'(phi) (z - cot(theta) m). ++ float limit = b3MotorJointSwingLimit( joint, n.x, n.y ); ++ float a = n.x >= 0.0f ? joint->upperSwingX : -joint->lowerSwingX; ++ float b = n.y >= 0.0f ? joint->upperSwingY : -joint->lowerSwingY; ++ a = b3MaxFloat( a, 1.0e-3f ); ++ b = b3MaxFloat( b, 1.0e-3f ); ++ float slope = limit * limit * limit * n.x * n.y * ( 1.0f / ( a * a ) - 1.0f / ( b * b ) ); ++ if ( limit >= 0.99f * B3_PI ) ++ { ++ slope = 0.0f; ++ } ++ float cotTheta = b3ClampFloat( cosf( swingAngle ) / b3MaxFloat( sinf( swingAngle ), 1.0e-3f ), -10.0f, 10.0f ); ++ b3Vec3 jacobianLocal = { ++ -n.x - slope * cotTheta * m.x, ++ -n.y - slope * cotTheta * m.y, ++ slope, ++ }; ++ b3Vec3 jacobian = b3RotateVector( limitA, jacobianLocal ); ++ float k = b3Dot( jacobian, b3MulMV( invInertiaSum, jacobian ) ); ++ float swingMass = k > 0.0f ? 1.0f / k : 0.0f; ++ joint->swingAxis = b3Neg( jacobian ); ++ ++ float c = limit - swingAngle; ++ float bias = 0.0f; ++ float massScale = 1.0f; ++ float impulseScale = 0.0f; ++ if ( c > 0.0f ) ++ { ++ // speculation ++ bias = c * context->inv_h; ++ } ++ else if ( useBias ) ++ { ++ bias = base->constraintSoftness.biasRate * c; ++ massScale = base->constraintSoftness.massScale; ++ impulseScale = base->constraintSoftness.impulseScale; ++ } ++ ++ float cdot = b3Dot( b3Sub( wB, wA ), jacobian ); ++ float oldImpulse = joint->swingImpulse; ++ float deltaImpulse = -massScale * swingMass * ( cdot + bias ) - impulseScale * oldImpulse; ++ joint->swingImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); ++ deltaImpulse = joint->swingImpulse - oldImpulse; ++ ++ wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, jacobian ) ); ++ wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, jacobian ) ); ++ } ++ else ++ { ++ // at the centre the swing direction is undefined (and the limit far away) ++ joint->swingImpulse = 0.0f; ++ } ++ ++ if ( joint->enableTwistLimit ) ++ { ++ float twistAngle = b3GetTwistAngle( limitQ ); ++ ++ // Exact Jacobian of twist = 2 atan2(q.z, q.s) for dq/dt = 0.5 (0, w) * q, w in frame A ++ // (the spherical joint's tan(theta/2) form lags by ~5 degrees at 100 degrees of swing). ++ float qs = limitQ.s, qx = limitQ.v.x, qy = limitQ.v.y, qz = limitQ.v.z; ++ float invD = 1.0f / b3MaxFloat( qs * qs + qz * qz, 1.0e-6f ); ++ b3Vec3 twistLocal = { ( qs * qy + qz * qx ) * invD, ( qz * qy - qs * qx ) * invD, 1.0f }; ++ b3Vec3 twistJacobian = b3RotateVector( limitA, twistLocal ); ++ float k = b3Dot( twistJacobian, b3MulMV( invInertiaSum, twistJacobian ) ); ++ float twistMass = k > 0.0f ? 1.0f / k : 0.0f; ++ joint->twistJacobian = twistJacobian; ++ ++ // Lower limit ++ { ++ float c = twistAngle - joint->lowerTwistAngle; ++ float bias = 0.0f; ++ float massScale = 1.0f; ++ float impulseScale = 0.0f; ++ if ( c > 0.0f ) ++ { ++ // speculation ++ bias = c * context->inv_h; ++ } ++ else if ( useBias ) ++ { ++ bias = base->constraintSoftness.biasRate * c; ++ massScale = base->constraintSoftness.massScale; ++ impulseScale = base->constraintSoftness.impulseScale; ++ } ++ ++ float cdot = b3Dot( b3Sub( wB, wA ), twistJacobian ); ++ float oldImpulse = joint->lowerTwistImpulse; ++ float deltaImpulse = -massScale * twistMass * ( cdot + bias ) - impulseScale * oldImpulse; ++ joint->lowerTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); ++ deltaImpulse = joint->lowerTwistImpulse - oldImpulse; ++ ++ wA = b3MulSub( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); ++ wB = b3MulAdd( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); ++ } ++ ++ // Upper limit ++ { ++ float c = joint->upperTwistAngle - twistAngle; ++ float bias = 0.0f; ++ float massScale = 1.0f; ++ float impulseScale = 0.0f; ++ if ( c > 0.0f ) ++ { ++ // speculation ++ bias = c * context->inv_h; ++ } ++ else if ( useBias ) ++ { ++ bias = base->constraintSoftness.biasRate * c; ++ massScale = base->constraintSoftness.massScale; ++ impulseScale = base->constraintSoftness.impulseScale; ++ } ++ ++ // sign flipped on Cdot ++ float cdot = b3Dot( b3Sub( wA, wB ), twistJacobian ); ++ float oldImpulse = joint->upperTwistImpulse; ++ float deltaImpulse = -massScale * twistMass * ( cdot + bias ) - impulseScale * oldImpulse; ++ joint->upperTwistImpulse = b3MaxFloat( oldImpulse + deltaImpulse, 0.0f ); ++ deltaImpulse = joint->upperTwistImpulse - oldImpulse; ++ ++ // sign flipped on applied impulse ++ wA = b3MulAdd( wA, deltaImpulse, b3MulMV( iA, twistJacobian ) ); ++ wB = b3MulSub( wB, deltaImpulse, b3MulMV( iB, twistJacobian ) ); ++ } ++ } ++ } ++ + b3Vec3 rA = b3RotateVector( stateA->deltaRotation, joint->frameA.p ); + b3Vec3 rB = b3RotateVector( stateB->deltaRotation, joint->frameB.p ); + +diff -ruN a/src/recording.c b/src/recording.c +--- a/src/recording.c 2026-10-01 17:24:25.938994483 +0300 ++++ b/src/recording.c 2026-10-02 17:18:33.365120096 +0300 +@@ -327,7 +327,7 @@ + "b3DistanceJointDef changed: update b3RecW_DISTANCEJOINTDEF and its reader together" ); + _Static_assert( sizeof( void* ) != 8 || sizeof( b3FilterJointDef ) == 112, + "b3FilterJointDef changed: update b3RecW_FILTERJOINTDEF and its reader together" ); +-_Static_assert( sizeof( void* ) != 8 || sizeof( b3MotorJointDef ) == 168, ++_Static_assert( sizeof( void* ) != 8 || sizeof( b3MotorJointDef ) == 232, + "b3MotorJointDef changed: update b3RecW_MOTORJOINTDEF and its reader together" ); + _Static_assert( sizeof( void* ) != 8 || sizeof( b3PrismaticJointDef ) == 152, + "b3PrismaticJointDef changed: update b3RecW_PRISMATICJOINTDEF and its reader together" ); +@@ -464,6 +464,16 @@ + b3RecW_F32( buf, v.angularHertz ); + b3RecW_F32( buf, v.angularDampingRatio ); + b3RecW_F32( buf, v.maxSpringTorque ); ++ b3RecW_QUAT( buf, v.limitRotationA ); ++ b3RecW_QUAT( buf, v.limitRotationB ); ++ b3RecW_BOOL( buf, v.enableSwingLimit ); ++ b3RecW_F32( buf, v.lowerSwingX ); ++ b3RecW_F32( buf, v.upperSwingX ); ++ b3RecW_F32( buf, v.lowerSwingY ); ++ b3RecW_F32( buf, v.upperSwingY ); ++ b3RecW_BOOL( buf, v.enableTwistLimit ); ++ b3RecW_F32( buf, v.lowerTwistAngle ); ++ b3RecW_F32( buf, v.upperTwistAngle ); + } + + void b3RecW_PRISMATICJOINTDEF( b3RecBuffer* buf, b3PrismaticJointDef v ) +diff -ruN a/src/recording_replay.c b/src/recording_replay.c +--- a/src/recording_replay.c 2026-10-01 17:24:25.939994580 +0300 ++++ b/src/recording_replay.c 2026-10-02 17:18:19.922206121 +0300 +@@ -555,6 +555,16 @@ + def.angularHertz = b3RecR_F32( rdr ); + def.angularDampingRatio = b3RecR_F32( rdr ); + def.maxSpringTorque = b3RecR_F32( rdr ); ++ def.limitRotationA = b3RecR_QUAT( rdr ); ++ def.limitRotationB = b3RecR_QUAT( rdr ); ++ def.enableSwingLimit = b3RecR_BOOL( rdr ); ++ def.lowerSwingX = b3RecR_F32( rdr ); ++ def.upperSwingX = b3RecR_F32( rdr ); ++ def.lowerSwingY = b3RecR_F32( rdr ); ++ def.upperSwingY = b3RecR_F32( rdr ); ++ def.enableTwistLimit = b3RecR_BOOL( rdr ); ++ def.lowerTwistAngle = b3RecR_F32( rdr ); ++ def.upperTwistAngle = b3RecR_F32( rdr ); + return def; + } + diff --git a/src/joints/box3d_generic_6dof_joint_impl_3d.cpp b/src/joints/box3d_generic_6dof_joint_impl_3d.cpp index 8de5f8f..6687737 100644 --- a/src/joints/box3d_generic_6dof_joint_impl_3d.cpp +++ b/src/joints/box3d_generic_6dof_joint_impl_3d.cpp @@ -110,6 +110,34 @@ Vector3 Box3DGeneric6DOFJointImpl3D::_to_world_from_frame_a(const Vector3& p_loc return frame.xform(p_local); } +bool Box3DGeneric6DOFJointImpl3D::_drive_has_angular_limits() const { + for (const AxisState& a : axes) { + if (a.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT] && + a.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT] < a.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT]) { + return true; + } + } + return false; +} + +// Limit flag off = free; lower >= upper = locked (Godot's convention). +void Box3DGeneric6DOFJointImpl3D::_angular_range(int p_axis, float& r_lower, float& r_upper) const { + const AxisState& a = axes[p_axis]; + if (!a.flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]) { + r_lower = (float)(-0.99 * Math_PI); + r_upper = (float)(0.99 * Math_PI); + return; + } + const double lower = a.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_LOWER_LIMIT]; + const double upper = a.param[PhysicsServer3D::G6DOF_JOINT_ANGULAR_UPPER_LIMIT]; + if (lower >= upper) { + r_lower = r_upper = 0.0f; + return; + } + r_lower = (float)CLAMP(lower, -0.99 * Math_PI, 0.0); + r_upper = (float)CLAMP(upper, 0.0, 0.99 * Math_PI); +} + b3JointId Box3DGeneric6DOFJointImpl3D::_create_joint_id(b3WorldId p_world_id, b3BodyId p_body_a, b3BodyId p_body_b, b3Transform p_local_frame_a, b3Transform p_local_frame_b) { mode = _classify(); b3JointId id = b3_nullJointId; @@ -170,6 +198,24 @@ b3JointId Box3DGeneric6DOFJointImpl3D::_create_joint_id(b3WorldId p_world_id, b3 def.base.bodyIdB = p_body_b; def.base.localFrameA = godot_to_b3_transform(local_frame_a * Transform3D(Basis(_equilibrium_rotation()), _equilibrium_position())); def.base.localFrameB = p_local_frame_b; + if (_drive_has_angular_limits()) { + // Measured between the creation frames (not the equilibrium-shifted target frame), with + // the same twist remap as BALL: Box3D limit frame z = joint X, x = joint -Z, y = joint Y. + def.limitRotationA = remap_twist(p_local_frame_a).q; + def.limitRotationB = remap_twist(p_local_frame_b).q; + def.enableTwistLimit = axes[Vector3::AXIS_X].flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]; + _angular_range(Vector3::AXIS_X, def.lowerTwistAngle, def.upperTwistAngle); + def.enableSwingLimit = axes[Vector3::AXIS_Y].flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT] || + axes[Vector3::AXIS_Z].flag[PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT]; + float lower_z = 0.0f, upper_z = 0.0f; + _angular_range(Vector3::AXIS_Y, def.lowerSwingY, def.upperSwingY); + _angular_range(Vector3::AXIS_Z, lower_z, upper_z); + def.lowerSwingX = -upper_z; + def.upperSwingX = -lower_z; + // Stiffer than the 60 Hz joint default: an arm sliding along an asymmetric range at speed + // otherwise sinks ~5 degrees past the twist bound (the drive itself has its own springs). + def.base.constraintHertz = 120.0f; + } id = b3CreateMotorJoint(p_world_id, &def); if (_any_flag(PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT)) { for (int i = 0; i < 3; i++) { diff --git a/src/joints/box3d_generic_6dof_joint_impl_3d.hpp b/src/joints/box3d_generic_6dof_joint_impl_3d.hpp index af6d75d..528747b 100644 --- a/src/joints/box3d_generic_6dof_joint_impl_3d.hpp +++ b/src/joints/box3d_generic_6dof_joint_impl_3d.hpp @@ -17,6 +17,11 @@ // DRIVE — any linear axis not locked → b3MotorJoint: springs pull frame B onto frame A shifted // by the equilibrium points, force/torque capped by the motor force limits (0 = no // cap); motor target velocities add a velocity drive. Linear limits are not enforced. +// Angular limits ARE enforced (patched Box3D motor joint) once any angular axis has a +// real range (limit flag on, lower < upper): X = twist range, Y/Z = swing bounds of an +// asymmetric ellipse (swing rotation-vector components about joint Y and Z, lower and +// upper may differ), all measured between the joint frames AS CREATED — the equilibrium +// shift of frame A (the drive target) does not move them. A flag-off axis is free. // // Box3D units: spring STIFFNESS is the spring frequency in Hz and DAMPING the damping ratio // (not N/m and N·s/m) — Box3D's soft constraints are frequency-based and mass-independent. @@ -65,6 +70,8 @@ private: Vector3 _equilibrium_position() const; Vector3 _motor_velocity(Param p_param, Flag p_gate) const; Vector3 _to_world_from_frame_a(const Vector3& p_local) const; + bool _drive_has_angular_limits() const; + void _angular_range(int p_axis, float& r_lower, float& r_upper) const; void _apply_live(); AxisState axes[3]; -- 2.55.0