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

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed 2026-10-02 17:45:00 +03:00
1 parent 423b709cc9
commit 3e300c7f04
7 files changed
+826 -5

No files matched your search

+34 -2
View File
@@ -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