Shoulder range of motion: drive-mode angular limits in the Box3D fork

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

.vendor/box3d-drive-angular-limits.patch

@@ -0,0 +1,561 @@
+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
+

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.

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).

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.

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<Generic6DofJoint3D> configure = null)
 	{
 		Generic6DofJoint3D j = new() { Transform = at, NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true };
 		foreach (var flag in new System.Action<Generic6DofJoint3D.Flag, bool>[] { 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

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<float> Tick, float Duration, Func<bool> 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<string> _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)