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:
1 parent
423b709cc9
commit
3e300c7f04
7 files changed
+826
-5
No files matched your search
@@ -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
|
||||
|
||||
Reference in new issue
Block a user