diff --git a/.vendor/box3d-drive-angular-limits.patch b/.vendor/box3d-drive-angular-limits.patch new file mode 100644 index 0000000..0f7ec6c --- /dev/null +++ b/.vendor/box3d-drive-angular-limits.patch @@ -0,0 +1,561 @@ +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 + diff --git a/AGENTS.md b/AGENTS.md index 80f97fa..593bc04 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -27,7 +27,8 @@ embodiment/view system in particular is theory until proven. (rebuild recipe + source in `.vendor/`, only the Linux binary is patched locally). Patches so far: ConeTwistJoint3D → rigid weld (`research/godot-box3d-weld.md`), Generic6DOFJoint3D → ball/weld/drive with torque-capped muscle motors (`research/godot-box3d-6dof.md`, regression - `scenes/sixdof_spike.tscn`). + `scenes/sixdof_spike.tscn`), drive-mode angular limits = anatomical shoulder range (fork branch + `shoulder-cone-limit`, `.vendor/box3d-drive-angular-limits.patch`; a Box3D source patch applied by CMake). Jolt (built-in) is the fallback: reverting is the one project-setting line, and all gameplay code must stay standard-Godot-physics-node based so that flip is lossless. diff --git a/addons/godot-box3d/bin/libgodot-box3d.so b/addons/godot-box3d/bin/libgodot-box3d.so index e6b6b04..6362057 100644 Binary files a/addons/godot-box3d/bin/libgodot-box3d.so and b/addons/godot-box3d/bin/libgodot-box3d.so differ diff --git a/research/godot-box3d-6dof.md b/research/godot-box3d-6dof.md index 37d8b47..f86cbc5 100644 --- a/research/godot-box3d-6dof.md +++ b/research/godot-box3d-6dof.md @@ -17,7 +17,7 @@ the joint primitives for the Project-TX-style muscle ragdoll (`nimso-project-tx. |---|---|---| | all 3 linear axes locked (limit on, lower ≥ upper, no linear spring/motor), angular not fully locked or angular motor/spring on | `b3SphericalJoint` = **BALL** | joint **X = twist axis** (angular X limits = twist range); swing cone symmetric, half-angle = largest \|limit\| of angular Y/Z (warned if Y/Z differ); angular spring → rotation spring to the angular equilibrium (Euler, Godot default YXZ); angular **motor = torque-capped velocity motor** | | everything locked, no angular motor/spring (the Godot default) | `b3WeldJoint` = **WELD** | rigid at the creation pose | -| any linear axis free/springy/motored | `b3MotorJoint` = **DRIVE** | springs pull frame B onto frame A shifted by the equilibrium points (linear + Euler); force/torque caps = Linear/Angular **MotorForceLimit** (0 = uncapped); motor target velocities add a velocity drive; linear limits NOT enforced (warned) | +| any linear axis free/springy/motored | `b3MotorJoint` = **DRIVE** | springs pull frame B onto frame A shifted by the equilibrium points (linear + Euler); force/torque caps = Linear/Angular **MotorForceLimit** (0 = uncapped); motor target velocities add a velocity drive; linear limits NOT enforced (warned); **angular limits enforced** (patch 3, below) once any angular axis has a real range | - **Units differ from Godot/Jolt**: spring STIFFNESS = frequency in **Hz**, DAMPING = **damping ratio** (Box3D soft constraints are mass-independent). Choose ~5–15 Hz, ratio ~1. @@ -46,3 +46,46 @@ horizontal with a 10 N·m cap (0.0°) and FAILS with 1.5 N·m (drops 107°); tra moving target to 0.000 m; a hand driven into a wall never penetrates it and pushes the 10 kg anchor body AWAY (−0.82 m — push-off reaction; pulling toward a fixed point is the climbing case); all-locked = rigid (0.13°); 90 live flag toggles stay finite. All existing spikes (weld, physics, grab, body, collapse, arm) PASS 3× on the new binary. + +## Patch 3 (2026-10-02): angular limits on the DRIVE — the shoulder range of motion + +Problem: the shoulder is a DRIVE (linear motor = moving girdle socket, angular motor = muscle), and Box3D's +`b3MotorJoint` had no angular limits — an external force could bend the arm into the chest or behind the back. +A second joint can't add them (`b3SphericalJoint` always pins its anchor and would fight the moving socket), so the +limits went into Box3D's motor joint itself. + +- Fork branch `shoulder-cone-limit` (on `generic-6dof-joint`), commit "Angular limits for drive-mode + Generic6DOFJoint3D". The Box3D change is `patches/box3d/0001-motor-joint-angular-limits.patch` in the fork, applied + to the `box3d/` checkout by CMake at configure time (marker `b3MotorJointSwingLimit`, idempotent) — the submodule + stays pinned to upstream. Rebuild recipe unchanged (`cmake -B build …`, `cmake --build build`). Linux binary only. +- Box3D side (`b3MotorJointDef`): `limitRotationA/B` (own limit frames, so moving the drive target never moves the + range), `enableSwingLimit` + `lower/upperSwingX/Y` (per-quadrant ELLIPSE on the swing rotation vector — asymmetric + bounds), `enableTwistLimit` + `lower/upperTwistAngle`. Solved after the angular motor/spring, Jacobians rebuilt + every sub-step: swing `J = −n + r′(φ)(z − cot θ · m)` (the radius change along an eccentric boundary — without + it a fast arm sank 36° into the corners), exact twist Jacobian (Box3D's spherical `tan(θ/2)` form lags ~5° at 100° + swing). Warm-started, included in joint torque reports and recordings (def size tripwire 168 → 232). +- Wrapper mapping: a DRIVE gets limits when any angular axis has the limit flag on with lower < upper. Joint + **X = twist** (X limits), **Y/Z = swing bounds**: the swing rotation-vector components about joint Y and Z must stay + inside the ellipse whose semi-axes are the per-sign bounds (lower ≤ 0 ≤ upper, up to ±0.99π). Flag off = that axis + free; lower ≥ upper = locked. Measured between the joint frames AS CREATED (relative rotation identity = centre), + not the equilibrium-shifted target. Limit softness 120 Hz (joint default 60 Hz let the twist sink ~5° while the arm + slid along the swing boundary under 25 N·m). Limits rebuild the joint — configure before `AddChild`. +- Plain drives (angular limit flags off, or Godot's locked default with no real range) are byte-for-byte unchanged: + `ragdoll_spike` prints identical numbers on the old and new binary. + +### Shoulder values (for `PlayerRagdoll`) + +Joint frame at build: X = the humerus as built (`dir`), Y = world up made square to it, Z = X × Y. Fitted against +target arm directions (chest frame): bounds Y ∈ [−90°, +100°] (right arm; left mirrors to [−100°, +90°]), +Z ∈ [−85°, +155°], twist ±80°. Inside with ≥ 2.6° margin: hanging, 45° extension, forward, overhead, overhead 10° +behind, lateral, lateral 30° behind, 45°-across hug, hand on the opposite shoulder, hand behind the back, up-and-across. +Out by ≥ 8°: medial horizontal, 80° extension, straight back, behind-up, behind the head across, medial behind-down. +(Arm hanging medially in front of the torso stays allowed — real, and own-body collision is off anyway.) + +### Proof (added to `sixdof_spike`, 3× identical; weld_spike 3× PASS on the same binary) + +Shoulder-style drive (2 kg × 0.3 m arm, socket = linear velocity motor 2000 N, bounds Y [−20°, 60°], Z [−40°, 100°], +twist ±45°): a 25 N·m torque sweeping every swing direction + 6 N·m alternating twist → swing overshoot 1.1° max, +every bound reached (61/20/101/41°), twist max 46.3°, socket 1.8 mm. A 30 N·m muscle PD reaches targets inside the +range (0.0 / 0.0 / 1.3° error) and stops at 60.2° when commanded to 85° (beyond the 60° bound). Moving the socket +5.8 cm (shrug) tracks to 0.0 mm with the arm still (0.0° swing). diff --git a/research/ragdoll-player-step2.md b/research/ragdoll-player-step2.md index 9f59dca..880b3f6 100644 --- a/research/ragdoll-player-step2.md +++ b/research/ragdoll-player-step2.md @@ -219,3 +219,5 @@ fall); held poses drift 1 cm with the eye within 7 mm; crawl, mantle, climb, pul | **Stick crouch (right stick Y)** = `RagdollDriver.StickCrouch`, a virtual floor this far above the real one, fed to the solver, ground control and the view re-centering | The pose crouches, the play space settles that far below the ground, the camera rides down. Spike: 0.6 m → eye 1.045, origin −0.601, eye vs camera 5 mm; back up to 1.650 | | **XR origin drawn interpolated** (`PlayerRig`: physics owns `_physOrigin`; `_Process` (priority −100) shows `prev.InterpolateWith(cur, Engine.GetPhysicsInterpolationFraction())`, `_PhysicsProcess` (priority −100) restores it; anyone else moving the origin is adopted; jumps/snap turns don't interpolate) | The bodies are drawn interpolated a tick behind while the origin stepped raw at 144 Hz under a 90 Hz display — the body looked like it was in two places when moving (owner). Not verifiable headless; needs the headset | | Menu summon on left X only | Right A is jump | + +**Shoulder range (2026-10-02):** the fork's DRIVE now honours angular limits (Box3D motor joint + swing ellipse/twist, `research/godot-box3d-6dof.md`); `PlayerRagdoll.ShoulderRange` (Y −90..+100° mirrored, Z −85..+155°, twist ±80°, frame X = humerus at build). Ragdoll spike unchanged with it on — the limits only engage when something forces the arm. diff --git a/scripts/PlayerRagdoll.cs b/scripts/PlayerRagdoll.cs index 913b90b..de077a9 100644 --- a/scripts/PlayerRagdoll.cs +++ b/scripts/PlayerRagdoll.cs @@ -321,7 +321,8 @@ public partial class PlayerRagdoll : Node3D a.Hand.ContactMonitor = true; a.Hand.MaxContactsReported = 8; - a.Shoulder = MakeDrive(Chest, a.Upper, c * new Transform3D(Basis.Identity, shoulder), GirdleForce, ShoulderTorque); + a.Shoulder = MakeDrive(Chest, a.Upper, c * new Transform3D(ShoulderFrame(dir), shoulder), GirdleForce, ShoulderTorque, + j => ShoulderRange(j, sign)); a.Elbow = MakeHinge(a.Upper, a.Fore, c * elbow, c.Basis * new Basis(dir, hinge.Cross(dir), hinge)); a.Wrist = MakeBall(a.Fore, a.Hand, c * wrist, c.Basis * TwistAlong(dir), Mathf.DegToRad(80f), Mathf.DegToRad(90f), 0f, 0f); a.Guide = MakeDrive(Chest, a.Hand, c * new Transform3D(handBasis, handOrigin), HandGuideForce, HandGuideTorque); @@ -412,7 +413,37 @@ public partial class PlayerRagdoll : Node3D return new Ball { Joint = j, Parent = parent, Child = child, FrameA = Rot(parent).Inverse() * jq, FrameB = Rot(child).Inverse() * jq, Kp = kp }; } - private Drive MakeDrive(PhysicsBody3D parent, RigidBody3D child, Transform3D at, float force, float torque) + // Shoulder joint frame: X = the humerus as built (the twist axis), Y = "up" square to it, Z = X × Y. + // The range below is measured from this build pose (relative rotation identity). + private static Basis ShoulderFrame(Vector3 bone) + { + Vector3 y = (Vector3.Up - bone * bone.Y).Normalized(); + return new Basis(bone, y, bone.Cross(y)); + } + + // Anatomical range (fork: drive-mode angular limits): swing bounds about joint Y/Z, an asymmetric + // ellipse fitted so hanging, 45° extension, overhead (+10° behind), lateral 30° behind, a 45°-across + // hug and a hand behind the back fit with ≥ 2.5° to spare, while straight back, 80° extension, + // behind-the-head and medial-horizontal stay ≥ 8° out. The left arm mirrors Y (sign = −1). + public static readonly float ShoulderSwingBack = Mathf.DegToRad(90f), ShoulderSwingAcross = Mathf.DegToRad(100f); + public static readonly float ShoulderSwingDown = Mathf.DegToRad(85f), ShoulderSwingUp = Mathf.DegToRad(155f); + public static readonly float ShoulderTwist = Mathf.DegToRad(80f); + + private static void ShoulderRange(Generic6DofJoint3D j, float sign) + { + j.SetFlagX(Generic6DofJoint3D.Flag.EnableAngularLimit, true); + j.SetFlagY(Generic6DofJoint3D.Flag.EnableAngularLimit, true); + j.SetFlagZ(Generic6DofJoint3D.Flag.EnableAngularLimit, true); + j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -ShoulderTwist); + j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, ShoulderTwist); + j.SetParamY(Generic6DofJoint3D.Param.AngularLowerLimit, sign > 0f ? -ShoulderSwingBack : -ShoulderSwingAcross); + j.SetParamY(Generic6DofJoint3D.Param.AngularUpperLimit, sign > 0f ? ShoulderSwingAcross : ShoulderSwingBack); + j.SetParamZ(Generic6DofJoint3D.Param.AngularLowerLimit, -ShoulderSwingDown); + j.SetParamZ(Generic6DofJoint3D.Param.AngularUpperLimit, ShoulderSwingUp); + } + + private Drive MakeDrive(PhysicsBody3D parent, RigidBody3D child, Transform3D at, float force, float torque, + System.Action configure = null) { Generic6DofJoint3D j = new() { Transform = at, NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true }; foreach (var flag in new System.Action[] { j.SetFlagX, j.SetFlagY, j.SetFlagZ }) @@ -427,6 +458,7 @@ public partial class PlayerRagdoll : Node3D param(Generic6DofJoint3D.Param.LinearMotorForceLimit, force); param(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque); } + configure?.Invoke(j); // before AddChild: limits/flags rebuild the server joint AddChild(j); Quaternion jq = at.Basis.Orthonormalized().GetRotationQuaternion(); return new Drive diff --git a/scripts/SixDofSpike.cs b/scripts/SixDofSpike.cs index 33624c8..bcbae57 100644 --- a/scripts/SixDofSpike.cs +++ b/scripts/SixDofSpike.cs @@ -4,7 +4,8 @@ using Godot; // Headless regression for the forked addon's Generic6DOFJoint3D → Box3D mapping // (ball = b3SphericalJoint with cone/twist limits + torque-capped motor; drive = b3MotorJoint; -// all-locked = weld). Run first whenever the addon binary changes. +// all-locked = weld; drive + angular limits = b3MotorJoint with the fork's swing-ellipse/twist limits, +// the shoulder). Run first whenever the addon binary changes. // godot-mono --headless --fixed-fps 90 --path . scenes/sixdof_spike.tscn public partial class SixDofSpike : Node3D { @@ -18,6 +19,11 @@ public partial class SixDofSpike : Node3D private const float WeakMuscle = 1.5f; private const float MuscleKp = 20f; private const float MuscleMaxSpin = 8f; + // Limited drive (the shoulder): asymmetric swing bounds about joint Y / Z, twist about X. + private static readonly float LimLowerY = Mathf.DegToRad(-20f), LimUpperY = Mathf.DegToRad(60f); + private static readonly float LimLowerZ = Mathf.DegToRad(-40f), LimUpperZ = Mathf.DegToRad(100f); + private static readonly float LimTwist = Mathf.DegToRad(45f); + private const float SocketKp = 30f, SocketForce = 2000f; private readonly List<(string Name, Action Tick, float Duration, Func Finish)> _phases = new(); private int _phase = -1; @@ -37,6 +43,9 @@ public partial class SixDofSpike : Node3D _phases.Add(("hand pushing into a wall pushes the anchor body away (reaction)", DriveBlockedTick, 1.5f, DriveBlockedFinish)); _phases.Add(("all-locked config is rigid", WeldTick, 1.5f, WeldFinish)); _phases.Add(("live flag toggling stays finite", ToggleTick, 1f, ToggleFinish)); + _phases.Add(("limited drive holds the asymmetric swing ellipse + twist under hard torque", LimitedDriveTorqueTick, 4f, LimitedDriveTorqueFinish)); + _phases.Add(("limited drive: muscle reaches targets inside the range, stops at the bound outside it", LimitedDriveMuscleTick, 6f, LimitedDriveMuscleFinish)); + _phases.Add(("limited drive: the linear socket drive still moves the joint (shrug)", LimitedDriveSocketTick, 1.5f, LimitedDriveSocketFinish)); Next(); } @@ -210,6 +219,39 @@ public partial class SixDofSpike : Node3D _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0)); _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j)); break; + case 10: + case 11: + case 12: + // An upper arm (2 kg, 0.3 m) hanging from a static chest through a shoulder-style DRIVE: + // linear velocity motor = socket, angular velocity motor = muscle, angular limits on. + // Joint X points down the arm (twist axis), joint Y = world +X, joint Z = world +Z. + _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0)); + _box = MakeBox(new Vector3(0.08f, 0.3f, 0.08f), 2f, new Vector3(0, 2.85f, 0)); + _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => + { + j.Basis = new Basis(Vector3.Forward, Mathf.Pi / 2f); + ForAxes((f, p) => + { + f(Generic6DofJoint3D.Flag.EnableLinearLimit, false); + f(Generic6DofJoint3D.Flag.EnableAngularLimit, true); + f(Generic6DofJoint3D.Flag.EnableLinearMotor, true); + f(Generic6DofJoint3D.Flag.EnableMotor, true); + p(Generic6DofJoint3D.Param.LinearMotorForceLimit, SocketForce); + p(Generic6DofJoint3D.Param.AngularMotorForceLimit, phase == 11 ? 30f : 0.5f); + }, j); + j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -LimTwist); + j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, LimTwist); + j.SetParamY(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerY); + j.SetParamY(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperY); + j.SetParamZ(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerZ); + j.SetParamZ(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperZ); + }); + _overshootDeg = -999f; + _maxTwist = 0f; + _reach = Vector4.Zero; + _muscleStage = -1; + _stageErr.Clear(); + break; } } @@ -358,6 +400,146 @@ public partial class SixDofSpike : Node3D return _finite && _maxSwing < 2f && _maxPivotSep < PivotLimit; } + // ---- limited drive (shoulder) ---- + private float _overshootDeg; + private Vector4 _reach; // max +Y, max -Y, max +Z, max -Z swing components reached (deg) + private int _muscleStage; + private readonly List _stageErr = new(); + private bool _stagesOk = true; + + // Swing rotation vector (joint Y/Z components, rad) taking frame A's X onto frame B's X, and the + // remaining twist about X (rad). + private void SwingTwist(out Vector2 swing, out float twist) + { + Quaternion rel = Rel(); + Vector3 b = rel * Vector3.Right; + float angle = Vector3.Right.AngleTo(b); + Vector3 axis = Vector3.Right.Cross(b); + swing = axis.LengthSquared() > 1e-12f ? new Vector2(axis.Y, axis.Z).Normalized() * angle : Vector2.Zero; + Quaternion swingQ = axis.LengthSquared() > 1e-12f ? new Quaternion(axis.Normalized(), angle) : Quaternion.Identity; + Quaternion t = swingQ.Inverse() * rel; + twist = 2f * Mathf.Atan2(t.X, t.W); + if (twist > Mathf.Pi) twist -= Mathf.Tau; + if (twist < -Mathf.Pi) twist += Mathf.Tau; + } + + // Radius of the quadrant ellipse in the swing direction (same formula as the fork). + private static float SwingLimit(Vector2 dir) + { + float a = dir.X >= 0f ? LimUpperY : -LimLowerY, b = dir.Y >= 0f ? LimUpperZ : -LimLowerZ; + float u = dir.X / a, v = dir.Y / b; + return 1f / Mathf.Sqrt(u * u + v * v); + } + + // Socket: linear velocity motor toward the pivot (+ a moving offset), like PlayerRagdoll.ShoulderDrive. + private void SocketDrive(Vector3 offset) + { + Vector3 err = _pivotWorld + offset - _box.GlobalTransform * _pivotLocalB; + Vector3 v = FrameA().Inverse() * (err * SocketKp); + _joint.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X); + _joint.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y); + _joint.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z); + } + + private void Muscle(Quaternion target) + { + Vector3 err = BodyMath.RotationError(target, Rel()); + Vector3 w = (err * MuscleKp).LimitLength(MuscleMaxSpin); + _joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X); + _joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y); + _joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z); + } + + private void TrackLimits() + { + TrackFinite(); + SwingTwist(out Vector2 s, out float tw); + if (s.Length() > Mathf.DegToRad(2f)) + _overshootDeg = Mathf.Max(_overshootDeg, Mathf.RadToDeg(s.Length() - SwingLimit(s.Normalized()))); + _maxTwist = Mathf.Max(_maxTwist, Mathf.RadToDeg(Mathf.Abs(tw))); + _reach = new Vector4(Mathf.Max(_reach.X, Mathf.RadToDeg(s.X)), Mathf.Max(_reach.Y, -Mathf.RadToDeg(s.X)), + Mathf.Max(_reach.Z, Mathf.RadToDeg(s.Y)), Mathf.Max(_reach.W, -Mathf.RadToDeg(s.Y))); + _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep()); + } + + private void LimitedDriveTorqueTick(float dt) + { + SocketDrive(Vector3.Zero); + Muscle(Rel()); // weak (0.5 N·m) muscle holding nothing in particular + // A hard torque sweeping all swing directions (joint Y/Z) plus an alternating twist torque. + float phi = _t * 3f; + Vector3 local = new Vector3(6f * Mathf.Sign(Mathf.Sin(_t * 5f)), 25f * Mathf.Cos(phi), 25f * Mathf.Sin(phi)); + _box.ApplyTorque(FrameA() * local); + TrackLimits(); + } + + private bool LimitedDriveTorqueFinish() + { + GD.Print($"6DOF: 25 N·m sweep: swing overshoot max {_overshootDeg:F1} deg (limit {LimitToleranceDeg}); reached +Y {_reach.X:F0}/60, -Y {_reach.Y:F0}/20, +Z {_reach.Z:F0}/100, -Z {_reach.W:F0}/40 deg; twist max {_maxTwist:F1} deg (limit 45); socket {_maxPivotSep:F4} m"); + bool reached = _reach.X > 50f && _reach.Y > 14f && _reach.Z > 85f && _reach.W > 32f; + return _finite && _overshootDeg < LimitToleranceDeg && reached && _maxTwist < Mathf.RadToDeg(LimTwist) + LimitToleranceDeg && _maxPivotSep < PivotLimit; + } + + // Targets (swing Y, swing Z, twist, deg): inside, inside, inside, then one beyond the +Y bound. + private static readonly Vector3[] MuscleTargets = { new(40f, 0f, 0f), new(0f, 80f, 0f), new(-15f, -30f, 30f), new(85f, 0f, 0f) }; + + private static Quaternion SwingTwistQuat(Vector3 deg) + { + Vector3 rv = new(0f, Mathf.DegToRad(deg.X), Mathf.DegToRad(deg.Y)); + Quaternion swing = rv.LengthSquared() > 1e-12f ? new Quaternion(rv.Normalized(), rv.Length()) : Quaternion.Identity; + return swing * new Quaternion(Vector3.Right, Mathf.DegToRad(deg.Z)); + } + + private void LimitedDriveMuscleTick(float dt) + { + SocketDrive(Vector3.Zero); + int stage = Mathf.Min((int)(_t / 1.5f), MuscleTargets.Length - 1); + Quaternion target = SwingTwistQuat(MuscleTargets[stage]); + Muscle(target); + TrackLimits(); + if (_t >= (stage + 1) * 1.5f - dt * 1.5f && _muscleStage < stage) + { + _muscleStage = stage; + SwingTwist(out Vector2 s, out float tw); + Vector3 got = new(Mathf.RadToDeg(s.X), Mathf.RadToDeg(s.Y), Mathf.RadToDeg(tw)); + if (stage < 3) + { + float err = Mathf.RadToDeg(BodyMath.RotationError(target, Rel()).Length()); + _stageErr.Add($"target {MuscleTargets[stage]} → {got.X:F0},{got.Y:F0},{got.Z:F0} err {err:F1}"); + _stagesOk &= err < 3f; + } + else + { + _stageErr.Add($"target {MuscleTargets[stage]} (beyond +Y 60) → swing Y {got.X:F1}"); + _stagesOk &= got.X < 60f + LimitToleranceDeg && got.X > 55f; + } + } + } + + private bool LimitedDriveMuscleFinish() + { + GD.Print($"6DOF: 30 N·m muscle: {string.Join("; ", _stageErr)}; overshoot max {_overshootDeg:F1} deg; socket {_maxPivotSep:F4} m"); + return _finite && _stagesOk && _stageErr.Count == MuscleTargets.Length && _overshootDeg < LimitToleranceDeg && _maxPivotSep < PivotLimit; + } + + private Vector3 ShrugOffset => new Vector3(0f, 0.05f, -0.03f) * Mathf.Min(_t / 0.5f, 1f); + + private void LimitedDriveSocketTick(float dt) + { + SocketDrive(ShrugOffset); + Muscle(Quaternion.Identity); + TrackFinite(); + SwingTwist(out Vector2 s, out _); + _maxSwing = Mathf.Max(_maxSwing, Mathf.RadToDeg(s.Length())); + } + + private bool LimitedDriveSocketFinish() + { + float err = (_box.GlobalTransform * _pivotLocalB).DistanceTo(_pivotWorld + ShrugOffset); + GD.Print($"6DOF: socket moved {ShrugOffset.Length():F3} m: tracking error {err:F4} m (limit 0.005), arm swing max {_maxSwing:F1} deg"); + return _finite && err < 0.005f && _maxSwing < 5f; + } + private int _toggles; private void ToggleTick(float dt)