Foxyz/VR-Mess
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
b8365ee95fd60e70e380da8e13c463632063b4f3
VR-Mess/.vendor/box3d-drive-angular-limits.patch
T

562 lines
25 KiB
Diff
Raw Blame History

From 7303770df3f6c09741b5e463b83debc2b809b139 Mon Sep 17 00:00:00 2001
From: Foxyz <timka1302@gmail.com>
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 <noreply@anthropic.com>
---
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
Reference in new issue View Git Blame Copy Permalink