Body step 2: the player is a muscle-driven ragdoll (Project TX)
PlayerRagdoll: 9 bodies, spine/neck ball joints, shoulder drives (girdle + muscle), elbow hinges, torque-capped motor PDs toward the True Body, chest->hand guides, implicit balance motor, mass-scaled ground control. PlayerRig: XR driver with stick move / snap turn and Bonelab-style push-back (eye->camera sphere sweep). Replaces the chase bodies in main.tscn; RagdollSpike regression (14 phases). Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed
1 parent
d0b50fbc41
commit
d83273dd2f
16 files changed
+1487
-28
No files matched your search
@@ -20,6 +20,9 @@ public sealed class ArmSolver
|
||||
|
||||
public Vector3 Elbow { get; private set; }
|
||||
public Vector3 Wrist { get; private set; }
|
||||
// Unit vector from the arm axis toward the elbow (defined even for a straight arm); the
|
||||
// elbow hinge axis is Pole × (shoulder→wrist).
|
||||
public Vector3 Pole { get; private set; } = Vector3.Back;
|
||||
|
||||
// side = the arm's outward direction (−torso X for the left arm, +X for the right).
|
||||
// handBasis = the controller's basis (null → torso-frame reference only).
|
||||
@@ -78,7 +81,8 @@ public sealed class ArmSolver
|
||||
_poleAngle += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta);
|
||||
Vector3 pole = refN.Rotated(dir, _poleAngle);
|
||||
|
||||
Elbow = center + CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius;
|
||||
Pole = CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance);
|
||||
Elbow = center + Pole * radius;
|
||||
Wrist = Elbow + (wrist - Elbow).Normalized() * foreLen;
|
||||
}
|
||||
|
||||
|
||||
@@ -54,7 +54,7 @@ public partial class PhysicsHands : Node
|
||||
public static RigidBody3D GetBody(XRController3D controller)
|
||||
{
|
||||
if (_instance == null)
|
||||
return null;
|
||||
return PlayerRig.GetHandBody(controller); // the ragdoll player owns the hands
|
||||
return _instance._left == controller ? _instance._bodies[0] : _instance._bodies[1];
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,644 @@
|
||||
using Godot;
|
||||
using System.Collections.Generic;
|
||||
|
||||
// Body step 2 — the player IS a muscle-driven ragdoll (Nimso's Project TX, research/nimso-project-tx.md
|
||||
// §2): every bone drives ITSELF toward the True Body pose through torque-capped joint motors
|
||||
// (never world-pinned), the hands are additionally pulled to the controllers by a force-capped
|
||||
// guide that reacts into the chest, and "ground control" holds the pelvis up and carries the body
|
||||
// after the head. Input-agnostic: a driver (PlayerRig in-game, RagdollSpike headless) solves the
|
||||
// TrueBodySolver, then calls Step(); the camera stays 1:1 and the driver moves its play space by
|
||||
// OriginCorrection() when the body is blocked (push-back) or stands on higher/lower ground.
|
||||
//
|
||||
// Bodies (boxes, origin = centre of mass): pelvis (+ the not-yet-physical legs' mass), chest,
|
||||
// head (incl. neck), and per arm upper arm, forearm, hand. No light intermediate links: a 0.6 kg
|
||||
// neck or 0.8 kg clavicle squeezed between two strong motors oscillates under Box3D's
|
||||
// one-iteration-per-substep solver (measured: RagdollSpike, 2026-10-02).
|
||||
// Joints (fork Generic6DOFJoint3D mapping, research/godot-box3d-6dof.md):
|
||||
// spine, neck, wrist = BALL (cone/twist limits), spine/neck with muscle motors;
|
||||
// shoulder = DRIVE chest→upper arm: linear motor holds the humeral head on the (moving) girdle
|
||||
// point, angular motor = the shoulder muscle (no cone limit — Box3D's motor joint has none);
|
||||
// elbow = HingeJoint3D (0..150°) with a muscle motor;
|
||||
// hand guide = DRIVE chest→hand; balance = DRIVE world anchor→pelvis (angular only).
|
||||
// All motors are PDs written as target velocities each tick; the force/torque limits are the strength.
|
||||
public partial class PlayerRagdoll : Node3D
|
||||
{
|
||||
public const float BodyMass = 60f; // Nimso's character is 60 kg
|
||||
// Muscles: PD gain (1/s), max commanded spin (rad/s), torque cap (N·m).
|
||||
public const float SpineKp = 15f, SpineTorque = 400f;
|
||||
public const float NeckKp = 20f, NeckTorque = 60f;
|
||||
public const float ShoulderKp = 20f, ShoulderTorque = 45f; // ~human shoulder flexion strength
|
||||
public const float ElbowKp = 20f, ElbowTorque = 45f;
|
||||
public const float MuscleMaxSpin = 15f;
|
||||
public const float GirdleKp = 30f, GirdleForce = 2000f; // shoulder drive, linear part
|
||||
// Hand guide (chest→hand drive): pulls the hand onto the controller; capped so the arm,
|
||||
// not the guide, carries real loads.
|
||||
public const float HandGuideKp = 40f, HandGuideForce = 40f, HandGuideMaxSpeed = 10f;
|
||||
public const float HandGuideAngularKp = 40f, HandGuideTorque = 25f;
|
||||
// Ground control (world-space support on the pelvis, only while grounded).
|
||||
public const float GroundFrequency = 4f; // Hz
|
||||
public const float GroundDamping = 1f;
|
||||
public const float LegStrength = 2f; // max support, × body weight
|
||||
public const float GroundFriction = 1f; // horizontal force ≤ this × support
|
||||
public const float GroundProbeExtra = 0.3f; // ground counts while within hip height + this
|
||||
// Balance: an implicit torque-capped motor from a world anchor to the pelvis (explicit torques
|
||||
// let the arms' reaction torques set the whole trunk swinging).
|
||||
public const float BalanceKp = 10f;
|
||||
public const float BalanceTorque = 400f;
|
||||
public const float AirBalanceShare = 0.3f;
|
||||
// Play-space reconciliation.
|
||||
public const float CameraRadius = 0.03f; // sphere swept from the head's eye to the camera
|
||||
public const float FreeSlack = 0.2f; // camera may lead an unobstructed (lagging/held) head by this much
|
||||
public const float GroundFollowRate = 8f;
|
||||
public const float ResetDistance = 1.5f; // pelvis this far from its target → snap the whole body to the pose
|
||||
// Geometry.
|
||||
public const float LumbarFraction = 0.35f; // lumbar joint height above the hips, × trunk
|
||||
public const float PelvisLeanShare = 0.5f; // share of the crouch lean taken by the hips
|
||||
public static readonly Vector3 HeadCenterFromEye = new(0f, 0.02f, 0.08f);
|
||||
public static readonly Vector3 HeadSize = new(0.16f, 0.21f, 0.2f);
|
||||
public static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);
|
||||
public const float WristOffset = TrueBodySolver.WristOffset;
|
||||
public const float ElbowMaxFlex = 2.6f; // rad (~150°)
|
||||
// Sign of HingeJoint3D's angle/motor for a positive rotation of the child about the joint's
|
||||
// +Z (measured by RagdollSpike's "hinge sign" phase).
|
||||
public static float HingeSign = 1f;
|
||||
|
||||
// Collision layers (AGENTS.md): 3 body, 4 hands, 5 arms.
|
||||
private const uint BodyLayer = 0b100, HandLayer = 0b1000, ArmLayer = 0b10000;
|
||||
private const uint WorldMask = 0b1;
|
||||
|
||||
private sealed class Ball
|
||||
{
|
||||
public Generic6DofJoint3D Joint;
|
||||
public RigidBody3D Parent, Child;
|
||||
public Quaternion FrameA, FrameB; // joint frame relative to each body
|
||||
public float Kp;
|
||||
public Quaternion PrevTarget;
|
||||
public bool HasPrev;
|
||||
}
|
||||
|
||||
private sealed class Drive
|
||||
{
|
||||
public Generic6DofJoint3D Joint;
|
||||
public Quaternion FrameA, FrameB;
|
||||
public Vector3 AnchorA, AnchorB; // anchor point, parent-local / child-local
|
||||
public Quaternion PrevTarget;
|
||||
public bool HasPrev;
|
||||
}
|
||||
|
||||
private sealed class Arm
|
||||
{
|
||||
public RigidBody3D Upper, Fore, Hand;
|
||||
public Ball Wrist;
|
||||
public HingeJoint3D Elbow;
|
||||
public Drive Shoulder, Guide;
|
||||
public float UpperLen, ForeLen;
|
||||
public RigidBody3D Held;
|
||||
public float PrevFlexTarget = -1f;
|
||||
}
|
||||
|
||||
public RigidBody3D Pelvis { get; private set; }
|
||||
public RigidBody3D Chest { get; private set; }
|
||||
public RigidBody3D Head { get; private set; }
|
||||
public bool Built { get; private set; }
|
||||
public bool Grounded { get; private set; }
|
||||
public float GroundY { get; private set; }
|
||||
public Vector3 Eye => Head.GlobalTransform * _eyeLocal;
|
||||
public BodyProfile Profile { get; private set; }
|
||||
public int Snaps { get; private set; } // safety resets (NaN / too far) — should stay 0
|
||||
|
||||
private readonly Arm[] _arms = { new(), new() };
|
||||
private readonly List<RigidBody3D> _bodies = new();
|
||||
private Ball _spine, _neck;
|
||||
private Vector3 _neckBaseToEye; // head frame, canonical (neck straight)
|
||||
private float _trunk, _lumbar, _neckLen;
|
||||
private Vector3 _eyeLocal;
|
||||
private Vector3 _pelvisCenterLocal, _chestCenterLocal, _headCenterLocal; // from each body's proximal joint, in its basis
|
||||
private readonly Dictionary<RigidBody3D, Transform3D> _targets = new();
|
||||
private readonly Vector3[] _girdle = new Vector3[2]; // True Body shoulder offset, chest frame
|
||||
private Vector3 _pelvisTargetPrev;
|
||||
private bool _havePelvisPrev;
|
||||
|
||||
public RigidBody3D Hand(int side) => Built ? _arms[side].Hand : null;
|
||||
public RigidBody3D UpperArm(int side) => Built ? _arms[side].Upper : null;
|
||||
public RigidBody3D Forearm(int side) => Built ? _arms[side].Fore : null;
|
||||
public IReadOnlyList<RigidBody3D> Bodies => _bodies;
|
||||
|
||||
// ---------------------------------------------------------------- build
|
||||
|
||||
public void Build(BodyProfile p, TrueBodySolver pose, Transform3D left, Transform3D right)
|
||||
{
|
||||
Clear();
|
||||
Profile = p;
|
||||
_trunk = TrueBodySolver.TrunkRatio * p.Stature;
|
||||
_lumbar = LumbarFraction * _trunk;
|
||||
_neckLen = Mathf.Max(0.05f, p.ShoulderDrop + TrueBodySolver.EyeToNeckPivot.Y - TrueBodySolver.NeckBaseAboveShoulders);
|
||||
float m = BodyMass;
|
||||
float s = p.Stature / 1.75f;
|
||||
|
||||
// Canonical pose (standing, level head, arms straight down-forward-out) in frame C at the
|
||||
// current pelvis: joint cones/twists are centred on it and the elbow hinge's zero is a
|
||||
// straight arm. The body is then moved onto the live pose.
|
||||
Transform3D c = new(pose.YawBasis, pose.PelvisTarget);
|
||||
Vector3 hip = Vector3.Zero;
|
||||
Vector3 lumbar = new(0f, _lumbar, 0f);
|
||||
Vector3 upperChest = new(0f, _trunk, 0f);
|
||||
Vector3 neckBase = upperChest + new Vector3(0f, TrueBodySolver.NeckBaseAboveShoulders,
|
||||
-(TrueBodySolver.ShouldersBehindEyes - TrueBodySolver.EyeToNeckPivot.Z));
|
||||
Vector3 neckPivot = neckBase + new Vector3(0f, _neckLen, 0f);
|
||||
_eyeLocal = -TrueBodySolver.EyeToNeckPivot; // head body origin is set to its centre below
|
||||
Vector3 headCenterFromPivot = _eyeLocal + HeadCenterFromEye;
|
||||
|
||||
Vector3 pelvisSize = new(0.17f * p.Stature, _lumbar + 0.06f, 0.18f * s);
|
||||
_pelvisCenterLocal = new Vector3(0f, _lumbar * 0.5f - 0.03f, 0f);
|
||||
float chestHeight = _trunk - _lumbar + TrueBodySolver.NeckBaseAboveShoulders;
|
||||
Vector3 chestSize = new(2f * p.ShoulderHalfWidth * 0.75f, chestHeight, 0.2f * s);
|
||||
_chestCenterLocal = new Vector3(0f, chestHeight * 0.5f, 0f);
|
||||
_headCenterLocal = headCenterFromPivot;
|
||||
_eyeLocal -= headCenterFromPivot; // eye relative to the head body's centre
|
||||
|
||||
Color skin = new(0.85f, 0.72f, 0.6f), shirt = new(0.3f, 0.45f, 0.7f), pants = new(0.25f, 0.27f, 0.32f);
|
||||
Pelvis = MakeBody("Pelvis", new BoxShape3D { Size = pelvisSize }, (0.142f + 0.139f + 0.322f) * m, BodyLayer, WorldMask, pants,
|
||||
c * new Transform3D(Basis.Identity, hip + _pelvisCenterLocal));
|
||||
Chest = MakeBody("Chest", new BoxShape3D { Size = chestSize }, 0.216f * m, BodyLayer, WorldMask, shirt,
|
||||
c * new Transform3D(Basis.Identity, lumbar + _chestCenterLocal));
|
||||
Head = MakeBody("Head", new BoxShape3D { Size = HeadSize * s }, 0.081f * m, BodyLayer, WorldMask, skin,
|
||||
c * new Transform3D(Basis.Identity, neckPivot + headCenterFromPivot));
|
||||
// The neck: a box hanging under the head, same body.
|
||||
Vector3 neckCenter = (neckBase + neckPivot) * 0.5f - (neckPivot + headCenterFromPivot);
|
||||
Head.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter });
|
||||
MeshInstance3D neckMesh = new() { Mesh = new BoxMesh { Size = new Vector3(0.09f * s, _neckLen, 0.09f * s) }, Position = neckCenter };
|
||||
neckMesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = skin, Roughness = 0.6f });
|
||||
Head.AddChild(neckMesh);
|
||||
_neckBaseToEye = neckPivot - TrueBodySolver.EyeToNeckPivot - neckBase;
|
||||
|
||||
// World anchor for the balance motor (no shape, linear motor force 0 = position free).
|
||||
StaticBody3D anchor = new() { Name = "BalanceAnchor", CollisionLayer = 0, CollisionMask = 0 };
|
||||
AddChild(anchor);
|
||||
_balance = MakeDrive(anchor, Pelvis, Pelvis.GlobalTransform, 0f, BalanceTorque);
|
||||
_balanceCap = BalanceTorque;
|
||||
|
||||
_spine = MakeBall(Pelvis, Chest, c * lumbar, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(60f), Mathf.DegToRad(35f), SpineKp, SpineTorque);
|
||||
_neck = MakeBall(Chest, Head, c * neckBase, c.Basis * TwistAlong(Vector3.Up), Mathf.DegToRad(75f), Mathf.DegToRad(85f), NeckKp, NeckTorque);
|
||||
|
||||
for (int side = 0; side < 2; side++)
|
||||
{
|
||||
Arm a = _arms[side];
|
||||
float sign = side == 0 ? -1f : 1f;
|
||||
a.UpperLen = p.UpperArm;
|
||||
a.ForeLen = p.Forearm;
|
||||
Vector3 shoulder = upperChest + new Vector3(sign * p.ShoulderHalfWidth, 0f, 0f);
|
||||
Vector3 dir = new Vector3(sign * 0.42f, -0.74f, -0.47f).Normalized();
|
||||
Vector3 reference = Vector3.Down * 2f + Vector3.Back * 0.8f + new Vector3(sign * 0.3f, 0f, 0f);
|
||||
Vector3 pole = (reference - dir * reference.Dot(dir)).Normalized();
|
||||
Vector3 hinge = pole.Cross(dir).Normalized();
|
||||
Basis bone = BoneBasis(dir, hinge);
|
||||
Vector3 elbow = shoulder + dir * a.UpperLen;
|
||||
Vector3 wrist = elbow + dir * a.ForeLen;
|
||||
Vector3 handX = (Vector3.Right - dir * dir.X).Normalized();
|
||||
Basis handBasis = new(handX, (-dir).Cross(handX), -dir);
|
||||
Vector3 handOrigin = wrist + dir * WristOffset;
|
||||
string n = side == 0 ? "Left" : "Right";
|
||||
|
||||
a.Upper = MakeBody(n + "UpperArm", new BoxShape3D { Size = new Vector3(0.085f * s, a.UpperLen, 0.085f * s) }, 0.028f * m, ArmLayer, WorldMask, shirt,
|
||||
c * new Transform3D(bone, shoulder + dir * (a.UpperLen * 0.5f)));
|
||||
a.Fore = MakeBody(n + "Forearm", new BoxShape3D { Size = new Vector3(0.07f * s, a.ForeLen - 0.01f, 0.07f * s) }, 0.016f * m, ArmLayer, WorldMask, skin,
|
||||
c * new Transform3D(bone, elbow + dir * (a.ForeLen * 0.5f)));
|
||||
a.Hand = MakeBody(n + "Hand", new BoxShape3D { Size = HandSize }, 0.006f * m, HandLayer, WorldMask | HandLayer,
|
||||
side == 0 ? new Color(0.9f, 0.3f, 0.35f) : new Color(0.3f, 0.5f, 0.9f), c * new Transform3D(handBasis, handOrigin));
|
||||
a.Hand.ContinuousCd = true;
|
||||
a.Hand.ContactMonitor = true;
|
||||
a.Hand.MaxContactsReported = 8;
|
||||
|
||||
a.Shoulder = MakeDrive(Chest, a.Upper, c * new Transform3D(Basis.Identity, shoulder), GirdleForce, ShoulderTorque);
|
||||
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);
|
||||
}
|
||||
|
||||
Built = true;
|
||||
_havePelvisPrev = false;
|
||||
SnapToPose(pose, left, right);
|
||||
GD.Print($"PlayerRagdoll: built for stature {p.Stature:F2} m (arm {p.UpperArm:F3}+{p.Forearm:F3} m, {_bodies.Count} bodies)");
|
||||
}
|
||||
|
||||
public void Clear()
|
||||
{
|
||||
foreach (Node child in GetChildren())
|
||||
child.QueueFree();
|
||||
foreach (Node child in GetChildren())
|
||||
RemoveChild(child);
|
||||
_bodies.Clear();
|
||||
_targets.Clear();
|
||||
for (int i = 0; i < 2; i++)
|
||||
_arms[i] = new Arm();
|
||||
Built = false;
|
||||
}
|
||||
|
||||
// Twist axis (joint X) along `dir`.
|
||||
private static Basis TwistAlong(Vector3 dir)
|
||||
{
|
||||
Vector3 helper = Mathf.Abs(dir.Y) < 0.9f ? Vector3.Up : Vector3.Right;
|
||||
Vector3 z = dir.Cross(helper).Normalized();
|
||||
return new Basis(dir, z.Cross(dir), z);
|
||||
}
|
||||
|
||||
// Segment basis: Y along the bone, X = elbow hinge axis.
|
||||
private static Basis BoneBasis(Vector3 boneDir, Vector3 hinge)
|
||||
{
|
||||
Vector3 x = (hinge - boneDir * hinge.Dot(boneDir)).Normalized();
|
||||
return new Basis(x, boneDir, x.Cross(boneDir));
|
||||
}
|
||||
|
||||
private RigidBody3D MakeBody(string name, Shape3D shape, float mass, uint layer, uint mask, Color color, Transform3D at)
|
||||
{
|
||||
RigidBody3D body = new() { Name = name, Mass = mass, CollisionLayer = layer, CollisionMask = mask, Transform = at };
|
||||
body.AddChild(new CollisionShape3D { Shape = shape });
|
||||
if (shape is BoxShape3D box && layer != 0)
|
||||
{
|
||||
MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = box.Size } };
|
||||
mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.6f });
|
||||
body.AddChild(mesh);
|
||||
}
|
||||
AddChild(body);
|
||||
_bodies.Add(body);
|
||||
return body;
|
||||
}
|
||||
|
||||
private static Quaternion Rot(Node3D n) => n.GlobalTransform.Basis.Orthonormalized().GetRotationQuaternion();
|
||||
|
||||
// Configure BEFORE AddChild (the node pushes everything to the server on enter-tree).
|
||||
private Ball MakeBall(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame, float cone, float twist, float kp, float torque)
|
||||
{
|
||||
Generic6DofJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true };
|
||||
j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -twist);
|
||||
j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, twist);
|
||||
foreach (var set in new System.Action<Generic6DofJoint3D.Param, float>[] { j.SetParamY, j.SetParamZ })
|
||||
{
|
||||
set(Generic6DofJoint3D.Param.AngularLowerLimit, -cone);
|
||||
set(Generic6DofJoint3D.Param.AngularUpperLimit, cone);
|
||||
}
|
||||
if (torque > 0f)
|
||||
{
|
||||
j.SetFlagX(Generic6DofJoint3D.Flag.EnableMotor, true);
|
||||
j.SetFlagY(Generic6DofJoint3D.Flag.EnableMotor, true);
|
||||
j.SetFlagZ(Generic6DofJoint3D.Flag.EnableMotor, true);
|
||||
j.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
|
||||
j.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
|
||||
j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
|
||||
}
|
||||
AddChild(j);
|
||||
Quaternion jq = frame.Orthonormalized().GetRotationQuaternion();
|
||||
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)
|
||||
{
|
||||
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 })
|
||||
{
|
||||
flag(Generic6DofJoint3D.Flag.EnableLinearLimit, false);
|
||||
flag(Generic6DofJoint3D.Flag.EnableAngularLimit, false);
|
||||
flag(Generic6DofJoint3D.Flag.EnableLinearMotor, true);
|
||||
flag(Generic6DofJoint3D.Flag.EnableMotor, true);
|
||||
}
|
||||
foreach (var param in new System.Action<Generic6DofJoint3D.Param, float>[] { j.SetParamX, j.SetParamY, j.SetParamZ })
|
||||
{
|
||||
param(Generic6DofJoint3D.Param.LinearMotorForceLimit, force);
|
||||
param(Generic6DofJoint3D.Param.AngularMotorForceLimit, torque);
|
||||
}
|
||||
AddChild(j);
|
||||
Quaternion jq = at.Basis.Orthonormalized().GetRotationQuaternion();
|
||||
return new Drive
|
||||
{
|
||||
Joint = j,
|
||||
FrameA = Rot(parent).Inverse() * jq,
|
||||
FrameB = Rot(child).Inverse() * jq,
|
||||
AnchorA = parent.GlobalTransform.AffineInverse() * at.Origin,
|
||||
AnchorB = child.GlobalTransform.AffineInverse() * at.Origin,
|
||||
};
|
||||
}
|
||||
|
||||
private HingeJoint3D MakeHinge(RigidBody3D parent, RigidBody3D child, Vector3 at, Basis frame)
|
||||
{
|
||||
HingeJoint3D j = new() { Transform = new Transform3D(frame, at), NodeA = parent.GetPath(), NodeB = child.GetPath(), ExcludeNodesFromCollision = true };
|
||||
float lo = HingeSign > 0f ? 0f : -ElbowMaxFlex;
|
||||
float hi = HingeSign > 0f ? ElbowMaxFlex : 0f;
|
||||
j.SetFlag(HingeJoint3D.Flag.UseLimit, true);
|
||||
j.SetParam(HingeJoint3D.Param.LimitLower, lo);
|
||||
j.SetParam(HingeJoint3D.Param.LimitUpper, hi);
|
||||
j.SetFlag(HingeJoint3D.Flag.EnableMotor, true);
|
||||
j.SetParam(HingeJoint3D.Param.MotorMaxImpulse, ElbowTorque);
|
||||
AddChild(j);
|
||||
return j;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- pose targets
|
||||
|
||||
// World transform every body should have in the True Body pose. The trunk is rebuilt top-down
|
||||
// through the physical chain (upper chest → lumbar → hips) so a leaning chest doesn't drag
|
||||
// the head off the camera; the hips take part of the crouch lean (hip hinge).
|
||||
private void ComputeTargets(TrueBodySolver s, Transform3D left, Transform3D right)
|
||||
{
|
||||
Basis chest = s.ChestBasis;
|
||||
Basis pelvis = s.YawBasis * new Basis(Vector3.Right, -s.Lean * PelvisLeanShare);
|
||||
Vector3 lumbar = s.UpperChest - chest.Y * (_trunk - _lumbar);
|
||||
Vector3 hip = lumbar - pelvis.Y * _lumbar;
|
||||
_targets[Pelvis] = new Transform3D(pelvis, hip + pelvis * _pelvisCenterLocal);
|
||||
_targets[Chest] = new Transform3D(chest, lumbar + chest * _chestCenterLocal);
|
||||
// Head + neck pivot at the neck base: orient like the camera, tilted by the small extra
|
||||
// rotation that puts the body's eye on the camera (the True Body bends the neck at two points).
|
||||
Vector3 eyeDir = s.HeadBasis * _neckBaseToEye;
|
||||
Vector3 wantDir = s.Eye - s.NeckBase;
|
||||
Quaternion tilt = eyeDir.LengthSquared() > 1e-8f && wantDir.LengthSquared() > 1e-8f
|
||||
? new Quaternion(eyeDir.Normalized(), wantDir.Normalized()) : Quaternion.Identity;
|
||||
Basis head = new Basis(tilt) * s.HeadBasis;
|
||||
_targets[Head] = new Transform3D(head, s.NeckBase + head * (_neckBaseToEye - _eyeLocal));
|
||||
|
||||
for (int side = 0; side < 2; side++)
|
||||
{
|
||||
Arm a = _arms[side];
|
||||
Vector3 sh = s.Shoulder[side], el = s.Elbow[side], wr = s.Wrist[side];
|
||||
Vector3 armDir = wr - sh;
|
||||
armDir = armDir.LengthSquared() > 1e-8f ? armDir.Normalized() : chest * Vector3.Down;
|
||||
Vector3 hinge = s.Pole[side].Cross(armDir);
|
||||
hinge = hinge.LengthSquared() > 1e-8f ? hinge.Normalized() : chest.X;
|
||||
Vector3 up = (el - sh).Normalized();
|
||||
Vector3 fore = (wr - el).Normalized();
|
||||
_girdle[side] = chest.Transposed() * (sh - (s.UpperChest + chest.X * ((side == 0 ? -1f : 1f) * Profile.ShoulderHalfWidth)));
|
||||
_targets[a.Upper] = new Transform3D(BoneBasis(up, hinge), sh + up * (a.UpperLen * 0.5f));
|
||||
_targets[a.Fore] = new Transform3D(BoneBasis(fore, hinge), el + fore * (a.ForeLen * 0.5f));
|
||||
_targets[a.Hand] = (side == 0 ? left : right).Orthonormalized();
|
||||
}
|
||||
}
|
||||
|
||||
public Transform3D Target(RigidBody3D body) => _targets.TryGetValue(body, out Transform3D t) ? t : body.GlobalTransform;
|
||||
|
||||
// ---------------------------------------------------------------- per tick
|
||||
|
||||
public void Step(TrueBodySolver s, Transform3D left, Transform3D right, Vector3 leftVelocity, Vector3 rightVelocity, float floorY, float dt)
|
||||
{
|
||||
if (!Built)
|
||||
return;
|
||||
ComputeTargets(s, left, right);
|
||||
foreach (RigidBody3D b in _bodies)
|
||||
{
|
||||
if (BodyMath.ResetIfNonFinite(b, Target(b), "PlayerRagdoll"))
|
||||
{
|
||||
Snaps++;
|
||||
SnapToPose(s, left, right);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Transform3D pelvisTarget = _targets[Pelvis];
|
||||
if (Pelvis.GlobalPosition.DistanceTo(pelvisTarget.Origin) > ResetDistance)
|
||||
{
|
||||
GD.Print("PlayerRagdoll: body too far from the pose, snapped to it");
|
||||
Snaps++;
|
||||
SnapToPose(s, left, right);
|
||||
return;
|
||||
}
|
||||
|
||||
Muscle(_spine, _targets[Pelvis].Basis, _targets[Chest].Basis, dt);
|
||||
Muscle(_neck, _targets[Chest].Basis, _targets[Head].Basis, dt);
|
||||
for (int side = 0; side < 2; side++)
|
||||
{
|
||||
Arm a = _arms[side];
|
||||
ShoulderDrive(a, side, dt);
|
||||
ElbowMuscle(a, dt);
|
||||
HandGuide(a, _targets[a.Hand], side == 0 ? leftVelocity : rightVelocity);
|
||||
}
|
||||
GroundControl(pelvisTarget, floorY, dt);
|
||||
}
|
||||
|
||||
// Muscle = PD on the joint's relative rotation toward the relative rotation of the two TARGET
|
||||
// bases (a local pose, not a world pin), written as the motor's target velocity in frame A.
|
||||
// Feed-forward = the target's own relative spin, so a moving pose isn't chased with a lag of
|
||||
// speed/Kp.
|
||||
private static void Muscle(Ball b, Basis parentTarget, Basis childTarget, float dt)
|
||||
{
|
||||
Quaternion current = (Rot(b.Parent) * b.FrameA).Inverse() * (Rot(b.Child) * b.FrameB);
|
||||
Quaternion target = (parentTarget.Orthonormalized().GetRotationQuaternion() * b.FrameA).Inverse()
|
||||
* (childTarget.Orthonormalized().GetRotationQuaternion() * b.FrameB);
|
||||
Vector3 ff = b.HasPrev && dt > 0f ? (BodyMath.RotationError(target, b.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero;
|
||||
b.PrevTarget = target;
|
||||
b.HasPrev = true;
|
||||
Vector3 w = (ff + BodyMath.RotationError(target, current) * b.Kp).LimitLength(MuscleMaxSpin);
|
||||
SetAngularVelocity(b.Joint, w);
|
||||
}
|
||||
|
||||
private static void SetAngularVelocity(Generic6DofJoint3D j, Vector3 w)
|
||||
{
|
||||
j.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X);
|
||||
j.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y);
|
||||
j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z);
|
||||
}
|
||||
|
||||
private static void SetLinearVelocity(Generic6DofJoint3D j, Vector3 v)
|
||||
{
|
||||
j.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X);
|
||||
j.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y);
|
||||
j.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z);
|
||||
}
|
||||
|
||||
// Elbow flexion (0 = straight) about the upper arm's hinge axis.
|
||||
public float ElbowFlex(int side)
|
||||
{
|
||||
Arm a = _arms[side];
|
||||
Basis u = a.Upper.GlobalTransform.Basis.Orthonormalized();
|
||||
Vector3 f = a.Fore.GlobalTransform.Basis.Orthonormalized().Y;
|
||||
return Mathf.Atan2(u.X.Dot(u.Y.Cross(f)), u.Y.Dot(f));
|
||||
}
|
||||
|
||||
private void ElbowMuscle(Arm a, float dt)
|
||||
{
|
||||
int side = a == _arms[0] ? 0 : 1;
|
||||
Vector3 up = _targets[a.Upper].Basis.Y, fore = _targets[a.Fore].Basis.Y;
|
||||
float target = Mathf.Acos(Mathf.Clamp(up.Dot(fore), -1f, 1f));
|
||||
float ff = a.PrevFlexTarget >= 0f && dt > 0f ? Mathf.Clamp((target - a.PrevFlexTarget) / dt, -MuscleMaxSpin, MuscleMaxSpin) : 0f;
|
||||
a.PrevFlexTarget = target;
|
||||
float w = Mathf.Clamp(ff + (target - ElbowFlex(side)) * ElbowKp, -MuscleMaxSpin, MuscleMaxSpin);
|
||||
a.Elbow.SetParam(HingeJoint3D.Param.MotorTargetVelocity, HingeSign * w);
|
||||
}
|
||||
|
||||
// Shoulder = one drive: its linear motor keeps the humeral head on the chest's shoulder point
|
||||
// shifted by the True Body girdle offset (shrug/protraction), its angular motor is the
|
||||
// shoulder muscle (same PD + feed-forward as Muscle, parent = chest).
|
||||
private void ShoulderDrive(Arm a, int side, float dt)
|
||||
{
|
||||
Drive d = a.Shoulder;
|
||||
Transform3D chest = Chest.GlobalTransform.Orthonormalized();
|
||||
Vector3 desired = chest * (d.AnchorA + _girdle[side]);
|
||||
Vector3 err = desired - a.Upper.GlobalTransform * d.AnchorB;
|
||||
Quaternion frameA = chest.Basis.GetRotationQuaternion() * d.FrameA;
|
||||
SetLinearVelocity(d.Joint, frameA.Inverse() * (err * GirdleKp));
|
||||
|
||||
Quaternion current = frameA.Inverse() * (Rot(a.Upper) * d.FrameB);
|
||||
Quaternion target = (_targets[Chest].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameA).Inverse()
|
||||
* (_targets[a.Upper].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameB);
|
||||
Vector3 ff = d.HasPrev && dt > 0f ? (BodyMath.RotationError(target, d.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero;
|
||||
d.PrevTarget = target;
|
||||
d.HasPrev = true;
|
||||
SetAngularVelocity(d.Joint, (ff + BodyMath.RotationError(target, current) * ShoulderKp).LimitLength(MuscleMaxSpin));
|
||||
}
|
||||
|
||||
// Force-capped pull of the hand onto the controller, reacting into the chest (arm muscles).
|
||||
private void HandGuide(Arm a, Transform3D target, Vector3 targetVelocity)
|
||||
{
|
||||
Transform3D chest = Chest.GlobalTransform;
|
||||
Vector3 anchor = chest * a.Guide.AnchorA;
|
||||
Vector3 anchorVelocity = Chest.LinearVelocity + Chest.AngularVelocity.Cross(anchor - Chest.GlobalPosition);
|
||||
Vector3 desired = (targetVelocity + (target.Origin - a.Hand.GlobalPosition) * HandGuideKp).LimitLength(HandGuideMaxSpeed);
|
||||
Quaternion frameA = Rot(Chest) * a.Guide.FrameA;
|
||||
SetLinearVelocity(a.Guide.Joint, frameA.Inverse() * (desired - anchorVelocity));
|
||||
// Angular: hand rotation onto the controller rotation, as a relative spin vs the chest.
|
||||
Vector3 err = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(a.Hand));
|
||||
Vector3 wWorld = (err * HandGuideAngularKp).LimitLength(MuscleMaxSpin * 2f) - Chest.AngularVelocity;
|
||||
SetAngularVelocity(a.Guide.Joint, frameA.Inverse() * wWorld);
|
||||
}
|
||||
|
||||
// "Leg strength": a stiff, mass-scaled support holding the pelvis at its pose height above the
|
||||
// ground and carrying it after the head horizontally (friction-limited), plus an upright/yaw
|
||||
// balance motor (world anchor → pelvis). No ground → no support (falls); balance weakens in the air.
|
||||
private void GroundControl(Transform3D target, float floorY, float dt)
|
||||
{
|
||||
Vector3 pos = Pelvis.GlobalPosition;
|
||||
float hipAboveFloor = target.Origin.Y - floorY;
|
||||
var query = PhysicsRayQueryParameters3D.Create(pos, pos + Vector3.Down * (hipAboveFloor + GroundProbeExtra + 0.5f), WorldMask);
|
||||
Godot.Collections.Dictionary hit = GetWorld3D().DirectSpaceState.IntersectRay(query);
|
||||
Grounded = false;
|
||||
if (hit.Count > 0)
|
||||
{
|
||||
Vector3 point = (Vector3)hit["position"];
|
||||
Vector3 normal = (Vector3)hit["normal"];
|
||||
if (pos.Y - point.Y < hipAboveFloor + GroundProbeExtra && normal.Y > 0.6f)
|
||||
{
|
||||
Grounded = true;
|
||||
GroundY = point.Y;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 targetVelocity = _havePelvisPrev && dt > 0f ? (target.Origin - _pelvisTargetPrev) / dt : Vector3.Zero;
|
||||
_pelvisTargetPrev = target.Origin;
|
||||
_havePelvisPrev = true;
|
||||
if (targetVelocity.Length() > 10f)
|
||||
targetVelocity = Vector3.Zero;
|
||||
|
||||
float weight = BodyMass * 9.8f;
|
||||
if (Grounded)
|
||||
{
|
||||
// Pose heights are relative to the tracking floor; stand on the ground actually under us.
|
||||
Vector3 goal = target.Origin + Vector3.Up * (GroundY - floorY);
|
||||
float w = Mathf.Tau * GroundFrequency;
|
||||
Vector3 accel = (goal - pos) * (w * w) + (targetVelocity - Pelvis.LinearVelocity) * (2f * GroundDamping * w);
|
||||
Vector3 force = accel * BodyMass;
|
||||
float support = Mathf.Clamp(force.Y + weight, 0f, LegStrength * weight);
|
||||
Vector3 horizontal = new Vector3(force.X, 0f, force.Z).LimitLength(GroundFriction * support);
|
||||
Pelvis.ApplyCentralForce(horizontal + Vector3.Up * support);
|
||||
}
|
||||
|
||||
Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(Pelvis));
|
||||
Vector3 spin = (rotErr * BalanceKp).LimitLength(MuscleMaxSpin);
|
||||
SetAngularVelocity(_balance.Joint, _balance.FrameA.Inverse() * spin); // anchor = world frame
|
||||
float cap = BalanceTorque * (Grounded ? 1f : AirBalanceShare);
|
||||
if (cap != _balanceCap)
|
||||
{
|
||||
_balanceCap = cap;
|
||||
_balance.Joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
|
||||
_balance.Joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
|
||||
_balance.Joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
|
||||
}
|
||||
}
|
||||
|
||||
private Drive _balance;
|
||||
private float _balanceCap;
|
||||
|
||||
// ---------------------------------------------------------------- play space
|
||||
|
||||
// How the driver should move its play-space origin after the last physics step (Bonelab-style
|
||||
// push-back): a sphere swept from the head body's eye to the camera — if geometry is in the way
|
||||
// the camera is somewhere the head can't follow (a wall, a bar, a table it walked into), so the
|
||||
// view is put back at the last free point. With nothing in between, the head may lag or be
|
||||
// held up to FreeSlack (leaning over a table with the hips blocked) before the view follows.
|
||||
// Plus: the floor follows the ground under the body; airborne, the view rides the body (falls).
|
||||
public Vector3 OriginCorrection(Vector3 camera, float floorY, float dt)
|
||||
{
|
||||
if (!Built)
|
||||
return Vector3.Zero;
|
||||
Vector3 eye = Eye;
|
||||
Vector3 lead = camera - eye;
|
||||
Vector3 correction = Vector3.Zero;
|
||||
if (lead.LengthSquared() > 1e-8f)
|
||||
{
|
||||
_cameraQuery ??= new PhysicsShapeQueryParameters3D { Shape = new SphereShape3D { Radius = CameraRadius }, CollisionMask = WorldMask };
|
||||
_cameraQuery.Transform = new Transform3D(Basis.Identity, eye);
|
||||
_cameraQuery.Motion = lead;
|
||||
float[] fractions = GetWorld3D().DirectSpaceState.CastMotion(_cameraQuery);
|
||||
float safe = fractions.Length > 0 ? fractions[0] : 1f;
|
||||
if (safe < 1f)
|
||||
correction = -lead * (1f - safe);
|
||||
else if (lead.Length() > FreeSlack)
|
||||
correction = -lead * (1f - FreeSlack / lead.Length());
|
||||
}
|
||||
if (Grounded)
|
||||
{
|
||||
correction.Y = 0f;
|
||||
correction.Y += (GroundY - floorY) * (1f - Mathf.Exp(-dt * GroundFollowRate));
|
||||
}
|
||||
return correction;
|
||||
}
|
||||
|
||||
private PhysicsShapeQueryParameters3D _cameraQuery;
|
||||
|
||||
public void TeleportBy(Transform3D delta)
|
||||
{
|
||||
if (!Built)
|
||||
return;
|
||||
foreach (RigidBody3D b in _bodies)
|
||||
{
|
||||
b.GlobalTransform = delta * b.GlobalTransform;
|
||||
b.LinearVelocity = delta.Basis * b.LinearVelocity;
|
||||
b.AngularVelocity = delta.Basis * b.AngularVelocity;
|
||||
}
|
||||
_pelvisTargetPrev = delta * _pelvisTargetPrev;
|
||||
}
|
||||
|
||||
public void SnapToPose(TrueBodySolver s, Transform3D left, Transform3D right)
|
||||
{
|
||||
ComputeTargets(s, left, right);
|
||||
foreach (RigidBody3D b in _bodies)
|
||||
{
|
||||
b.GlobalTransform = Target(b);
|
||||
b.LinearVelocity = Vector3.Zero;
|
||||
b.AngularVelocity = Vector3.Zero;
|
||||
}
|
||||
// The hands sit at the wrist end of the solved arm, not at a possibly unreachable controller.
|
||||
for (int side = 0; side < 2; side++)
|
||||
{
|
||||
Arm a = _arms[side];
|
||||
Basis hb = _targets[a.Hand].Basis;
|
||||
a.Hand.GlobalTransform = new Transform3D(hb, s.Wrist[side] - hb * new Vector3(0f, 0f, WristOffset));
|
||||
}
|
||||
_havePelvisPrev = false;
|
||||
}
|
||||
|
||||
// Collision exceptions between a held prop and that arm's segments (its hand is excluded by
|
||||
// the grab weld itself) — a forearm wrapped around a held object must not fight it.
|
||||
public void SetHeld(int side, RigidBody3D prop)
|
||||
{
|
||||
if (!Built)
|
||||
return;
|
||||
Arm a = _arms[side];
|
||||
if (a.Held != null && IsInstanceValid(a.Held) && a.Held != _arms[1 - side].Held)
|
||||
{
|
||||
a.Upper.RemoveCollisionExceptionWith(a.Held);
|
||||
a.Fore.RemoveCollisionExceptionWith(a.Held);
|
||||
}
|
||||
a.Held = prop;
|
||||
if (prop != null)
|
||||
{
|
||||
a.Upper.AddCollisionExceptionWith(prop);
|
||||
a.Fore.AddCollisionExceptionWith(prop);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
uid://dvwdt7eb4nsm2
|
||||
@@ -0,0 +1,205 @@
|
||||
using Godot;
|
||||
|
||||
// In-game driver of the ragdoll player (body step 2): XR input → True Body → PlayerRagdoll, plus
|
||||
// Boneworks-style stick locomotion and the play-space reconciliation. Runs everything in one
|
||||
// physics tick, in this order:
|
||||
// 1. discrete origin jumps made by other code (spawn, teleport markers) carry the body along;
|
||||
// 2. stick: left = move (head-relative), right X = turn (snap by default, SmoothTurn for smooth);
|
||||
// 3. the ragdoll's OriginCorrection (push-back at walls, floor follows the ground, falls);
|
||||
// 4. True Body solve from the corrected camera/controllers, then the ragdoll's muscles.
|
||||
// Waits for the calibrated BodyProfile like everything else; rebuilds on recalibration.
|
||||
public partial class PlayerRig : Node
|
||||
{
|
||||
public const float MoveSpeed = 2.2f; // m/s at full stick (Boneworks-ish walk)
|
||||
public const float MoveAcceleration = 8f; // m/s²
|
||||
public const float StickDeadzone = 0.2f;
|
||||
public static bool SmoothTurn = false;
|
||||
public const float SnapTurnDegrees = 30f;
|
||||
public const float SmoothTurnSpeed = 2.5f; // rad/s
|
||||
public const float SnapTurnThreshold = 0.7f;
|
||||
public const float SnapTurnRearm = 0.4f;
|
||||
public const float JumpDistance = 0.5f; // an origin move this big in one tick = teleport, not motion
|
||||
public const float JumpAngle = 0.1f; // rad
|
||||
private const float GhostLagDistance = 0.1f;
|
||||
|
||||
private static PlayerRig _instance;
|
||||
|
||||
private XROrigin3D _origin;
|
||||
private XRCamera3D _camera;
|
||||
private XRController3D _left, _right;
|
||||
private TrueBody _trueBody;
|
||||
private PlayerRagdoll _ragdoll;
|
||||
private Transform3D _lastOrigin;
|
||||
private bool _haveLastOrigin;
|
||||
private Vector3 _moveVelocity;
|
||||
private bool _snapArmed = true;
|
||||
private readonly Vector3[] _ctrlPrev = new Vector3[2];
|
||||
private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2];
|
||||
private bool _rebuild = true;
|
||||
|
||||
public PlayerRagdoll Ragdoll => _ragdoll;
|
||||
|
||||
public static RigidBody3D GetHandBody(XRController3D controller)
|
||||
{
|
||||
if (_instance == null || !_instance._ragdoll.Built)
|
||||
return null;
|
||||
return _instance._ragdoll.Hand(controller == _instance._left ? 0 : 1);
|
||||
}
|
||||
|
||||
public static void SetHeld(XRController3D controller, RigidBody3D prop)
|
||||
{
|
||||
if (_instance != null)
|
||||
_instance._ragdoll.SetHeld(controller == _instance._left ? 0 : 1, prop);
|
||||
}
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
_instance = this;
|
||||
_origin = GetNode<XROrigin3D>("../XROrigin3D");
|
||||
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
|
||||
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
|
||||
_right = GetNode<XRController3D>("../XROrigin3D/RightHand");
|
||||
_trueBody = GetNodeOrNull<TrueBody>("../TrueBody");
|
||||
if (_trueBody != null)
|
||||
{
|
||||
_trueBody.ExternallySolved = true;
|
||||
TrueBody.ShowDebug = false;
|
||||
}
|
||||
_ragdoll = new PlayerRagdoll { Name = "Ragdoll" };
|
||||
AddChild(_ragdoll);
|
||||
BodyProfile.Changed += _ => _rebuild = true;
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
_ghosts[i] = new MeshInstance3D
|
||||
{
|
||||
Mesh = new BoxMesh { Size = PlayerRagdoll.HandSize },
|
||||
Visible = false,
|
||||
CastShadow = GeometryInstance3D.ShadowCastingSetting.Off,
|
||||
};
|
||||
_ghosts[i].SetSurfaceOverrideMaterial(0, new StandardMaterial3D
|
||||
{
|
||||
AlbedoColor = new Color(1f, 1f, 1f, 0.3f),
|
||||
Transparency = BaseMaterial3D.TransparencyEnum.Alpha,
|
||||
ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
|
||||
});
|
||||
AddChild(_ghosts[i]);
|
||||
}
|
||||
}
|
||||
|
||||
private TrueBodySolver Solver => _trueBody != null ? _trueBody.Solver : _ownSolver;
|
||||
private readonly TrueBodySolver _ownSolver = new();
|
||||
|
||||
public override void _PhysicsProcess(double delta)
|
||||
{
|
||||
float dt = (float)delta;
|
||||
BodyProfile profile = BodyProfile.Current;
|
||||
if (profile == null)
|
||||
return;
|
||||
|
||||
// 1. Someone else moved the play space discretely: bring the body along.
|
||||
Transform3D origin = _origin.GlobalTransform;
|
||||
if (_haveLastOrigin && _ragdoll.Built && IsJump(_lastOrigin, origin))
|
||||
Carry(origin * _lastOrigin.AffineInverse());
|
||||
|
||||
if (_ragdoll.Built && !_rebuild)
|
||||
{
|
||||
// 2. Stick locomotion, 3. reconciliation.
|
||||
Locomote(dt);
|
||||
Vector3 correction = _ragdoll.OriginCorrection(_camera.GlobalPosition, _origin.GlobalPosition.Y, dt);
|
||||
if (correction != Vector3.Zero)
|
||||
_origin.GlobalPosition += correction;
|
||||
}
|
||||
|
||||
// 4. Pose + muscles.
|
||||
Transform3D head = _camera.GlobalTransform;
|
||||
Transform3D left = _left.GlobalTransform, right = _right.GlobalTransform;
|
||||
Solver.Solve(profile, head, left, right, _origin.GlobalPosition.Y, dt);
|
||||
if (_rebuild || !_ragdoll.Built)
|
||||
{
|
||||
_ragdoll.Build(profile, Solver, left, right);
|
||||
_rebuild = false;
|
||||
_ctrlPrev[0] = left.Origin;
|
||||
_ctrlPrev[1] = right.Origin;
|
||||
}
|
||||
Vector3 lv = (left.Origin - _ctrlPrev[0]) / dt, rv = (right.Origin - _ctrlPrev[1]) / dt;
|
||||
_ctrlPrev[0] = left.Origin;
|
||||
_ctrlPrev[1] = right.Origin;
|
||||
_ragdoll.Step(Solver, left, right, lv, rv, _origin.GlobalPosition.Y, dt);
|
||||
_lastOrigin = _origin.GlobalTransform;
|
||||
_haveLastOrigin = true;
|
||||
}
|
||||
|
||||
public override void _Process(double delta)
|
||||
{
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
XRController3D c = i == 0 ? _left : _right;
|
||||
RigidBody3D hand = _ragdoll.Built ? _ragdoll.Hand(i) : null;
|
||||
bool lagging = hand != null && hand.GlobalPosition.DistanceTo(c.GlobalPosition) > GhostLagDistance;
|
||||
_ghosts[i].Visible = lagging;
|
||||
if (lagging)
|
||||
_ghosts[i].GlobalTransform = c.GlobalTransform;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsJump(Transform3D a, Transform3D b) =>
|
||||
a.Origin.DistanceTo(b.Origin) > JumpDistance ||
|
||||
BodyMath.RotationError(b.Basis.GetRotationQuaternion(), a.Basis.GetRotationQuaternion()).Length() > JumpAngle;
|
||||
|
||||
private void Carry(Transform3D delta)
|
||||
{
|
||||
_ragdoll.TeleportBy(delta);
|
||||
Solver.Shift(delta);
|
||||
_ctrlPrev[0] = delta * _ctrlPrev[0];
|
||||
_ctrlPrev[1] = delta * _ctrlPrev[1];
|
||||
}
|
||||
|
||||
private void Locomote(float dt)
|
||||
{
|
||||
// A stick whose hand points at the menu scrolls it instead.
|
||||
Vector2 move = WorldPanel.IsPointing(_left) ? Vector2.Zero : _left.GetVector2("primary");
|
||||
Vector2 turn = WorldPanel.IsPointing(_right) ? Vector2.Zero : _right.GetVector2("primary");
|
||||
|
||||
// Move: head-relative on the ground plane, acceleration-limited. The body follows the
|
||||
// camera physically, so a wall stops it and the push-back stops the view.
|
||||
Vector3 desired = Vector3.Zero;
|
||||
if (move.Length() > StickDeadzone)
|
||||
{
|
||||
Vector3 fwd = -_camera.GlobalTransform.Basis.Z;
|
||||
fwd.Y = 0f;
|
||||
fwd = fwd.LengthSquared() > 1e-6f ? fwd.Normalized() : Vector3.Forward;
|
||||
Vector3 rightDir = fwd.Cross(Vector3.Up);
|
||||
float mag = Mathf.Clamp((move.Length() - StickDeadzone) / (1f - StickDeadzone), 0f, 1f);
|
||||
desired = (fwd * move.Y + rightDir * move.X).Normalized() * (mag * MoveSpeed);
|
||||
}
|
||||
if (!_ragdoll.Grounded)
|
||||
desired = _moveVelocity; // no air control
|
||||
_moveVelocity = _moveVelocity.MoveToward(desired, MoveAcceleration * dt);
|
||||
if (_moveVelocity != Vector3.Zero)
|
||||
_origin.GlobalPosition += _moveVelocity * dt;
|
||||
|
||||
// Turn about the head.
|
||||
float angle = 0f;
|
||||
if (SmoothTurn)
|
||||
{
|
||||
if (Mathf.Abs(turn.X) > StickDeadzone)
|
||||
angle = -turn.X * SmoothTurnSpeed * dt;
|
||||
}
|
||||
else if (_snapArmed && Mathf.Abs(turn.X) > SnapTurnThreshold)
|
||||
{
|
||||
angle = -Mathf.Sign(turn.X) * Mathf.DegToRad(SnapTurnDegrees);
|
||||
_snapArmed = false;
|
||||
}
|
||||
else if (Mathf.Abs(turn.X) < SnapTurnRearm)
|
||||
_snapArmed = true;
|
||||
if (angle != 0f)
|
||||
{
|
||||
Vector3 pivot = _camera.GlobalPosition;
|
||||
Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, angle), Vector3.Zero)
|
||||
* new Transform3D(Basis.Identity, -pivot);
|
||||
_origin.GlobalTransform = rot * _origin.GlobalTransform;
|
||||
if (!SmoothTurn)
|
||||
Carry(rot);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
uid://n62b8ehiuaeb
|
||||
@@ -69,6 +69,7 @@ public partial class PropGrabber : Node
|
||||
_joints[hand] = MakeWeld(handBody, prop, pivot, RigidWeldBias);
|
||||
_localAnchor[hand] = prop.GlobalTransform.AffineInverse() * pivot;
|
||||
_arms?.SetHeld(hand, prop);
|
||||
PlayerRig.SetHeld(controller, prop);
|
||||
GD.Print($"PropGrabber: grabbed '{prop.Name}' ({prop.Mass:0.###} kg, {(hand == 0 ? "left" : "right")}{(_held[other] == prop ? ", two-handed" : "")})");
|
||||
}
|
||||
}
|
||||
@@ -186,6 +187,7 @@ public partial class PropGrabber : Node
|
||||
private void DestroyJoint(int hand)
|
||||
{
|
||||
_arms?.SetHeld(hand, null);
|
||||
PlayerRig.SetHeld(hand == 0 ? _left : _right, null);
|
||||
if (_joints[hand] != null && IsInstanceValid(_joints[hand]))
|
||||
_joints[hand].QueueFree();
|
||||
_joints[hand] = null;
|
||||
|
||||
@@ -0,0 +1,476 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using Godot;
|
||||
|
||||
// Headless regression for body step 2 — the muscle-driven ragdoll player (PlayerRagdoll) driven
|
||||
// exactly like PlayerRig drives it, from a simulated play space (origin + head/controller poses).
|
||||
// godot-mono --headless --fixed-fps 90 --path . scenes/ragdoll_spike.tscn
|
||||
// Must print "RAGDOLL: overall PASS".
|
||||
public partial class RagdollSpike : Node3D
|
||||
{
|
||||
private const float EyeHeight = 1.65f;
|
||||
|
||||
private readonly List<(string Name, Action Setup, Action<float> Tick, float Duration, Func<bool> Finish)> _phases = new();
|
||||
private int _phase = -1;
|
||||
private float _t;
|
||||
private bool _allOk = true;
|
||||
private Node3D _stage;
|
||||
private PlayerRagdoll _ragdoll;
|
||||
private readonly TrueBodySolver _solver = new();
|
||||
private BodyProfile _profile;
|
||||
|
||||
// Simulated play space.
|
||||
private Transform3D _origin = Transform3D.Identity;
|
||||
private Transform3D _headLocal, _leftLocal, _rightLocal;
|
||||
private Vector3 _stickVelocity;
|
||||
private Vector3 _prevL, _prevR;
|
||||
private bool _needBuild;
|
||||
|
||||
// Measurements (reset per phase).
|
||||
private float _maxEyeErr, _maxHandErr, _endHandErr, _maxSpeed, _minCamZ, _minEyeZ, _maxElbowErr;
|
||||
private bool _finite;
|
||||
private int _snapsAtStart;
|
||||
private float _measureFrom;
|
||||
|
||||
private Transform3D Camera => _origin * _headLocal;
|
||||
private Transform3D Left => _origin * _leftLocal;
|
||||
private Transform3D Right => _origin * _rightLocal;
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
_profile = BodyProfile.Estimate(EyeHeight);
|
||||
AddChild(MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29));
|
||||
_ragdoll = new PlayerRagdoll { Name = "Ragdoll" };
|
||||
AddChild(_ragdoll);
|
||||
|
||||
_phases.Add(("hinge sign", SetupHinge, TickHinge, 0.3f, FinishHinge));
|
||||
_phases.Add(("stands still, head on the camera, hands on the controllers", Rest, _ => { }, 4f, FinishStand));
|
||||
_phases.Add(("reaches forward, out and overhead", Rest, TickReach, 4.5f, FinishReach));
|
||||
_phases.Add(("fast arm swings stay attached", Rest, TickSwing, 3f, FinishSwing));
|
||||
_phases.Add(("stick-walking into a wall is stopped (view pushed back)", SetupWall, _ => _stickVelocity = new Vector3(0f, 0f, -2f), 3f, FinishWall));
|
||||
_phases.Add(("room-scale walking into a wall is stopped", SetupWall, TickRoomWalk, 3f, FinishWall));
|
||||
_phases.Add(("standing walk into a low bar is blocked", SetupBar, _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 3f, FinishBarBlocked));
|
||||
_phases.Add(("crouched walk passes under the low bar", SetupBar, TickCrouchWalk, 4f, FinishBarPassed));
|
||||
_phases.Add(("leans over a table without being pushed back", SetupTable, TickLean, 3f, FinishLean));
|
||||
_phases.Add(("pushes a 10 kg crate", SetupCrate, TickPush, 3f, FinishCrate));
|
||||
_phases.Add(("12 kg held close to the body", () => SetupWeight(close: true), _ => { }, 3f, () => FinishWeight(close: true)));
|
||||
_phases.Add(("12 kg held at arm's length sags more", () => SetupWeight(close: false), _ => { }, 3f, () => FinishWeight(close: false)));
|
||||
_phases.Add(("teleport + snap turn carry the body", Rest, TickTeleport, 2f, FinishTeleport));
|
||||
_phases.Add(("random motion soak", Rest, TickRandom, 8f, FinishSoak));
|
||||
Next();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- harness
|
||||
|
||||
public override void _PhysicsProcess(double delta)
|
||||
{
|
||||
if (_phase >= _phases.Count)
|
||||
return;
|
||||
float dt = (float)delta;
|
||||
_t += dt;
|
||||
_phases[_phase].Tick(dt);
|
||||
if (_phase > 0)
|
||||
Drive(dt);
|
||||
if (_t >= _phases[_phase].Duration)
|
||||
{
|
||||
bool ok = _phases[_phase].Finish();
|
||||
_allOk &= ok;
|
||||
GD.Print($"RAGDOLL: {_phases[_phase].Name} → {(ok ? "PASS" : "FAIL")}");
|
||||
Next();
|
||||
}
|
||||
}
|
||||
|
||||
private void Next()
|
||||
{
|
||||
_stage?.QueueFree();
|
||||
_stage = new Node3D();
|
||||
AddChild(_stage);
|
||||
_t = 0f;
|
||||
_phase++;
|
||||
if (_phase >= _phases.Count)
|
||||
{
|
||||
GD.Print($"RAGDOLL: overall {(_allOk ? "PASS — muscle ragdoll player functional" : "CHECK — inspect failures above")}");
|
||||
GetTree().Quit();
|
||||
return;
|
||||
}
|
||||
_maxEyeErr = _maxHandErr = _endHandErr = _maxSpeed = _maxElbowErr = 0f;
|
||||
_minCamZ = _minEyeZ = float.MaxValue;
|
||||
_finite = true;
|
||||
_measureFrom = 1f;
|
||||
_stickVelocity = Vector3.Zero;
|
||||
_phases[_phase].Setup();
|
||||
_snapsAtStart = _ragdoll.Snaps;
|
||||
}
|
||||
|
||||
// Exactly PlayerRig's tick order.
|
||||
private void Drive(float dt)
|
||||
{
|
||||
_origin.Origin += _stickVelocity * dt;
|
||||
if (_ragdoll.Built && !_needBuild)
|
||||
_origin.Origin += _ragdoll.OriginCorrection(Camera.Origin, _origin.Origin.Y, dt);
|
||||
_solver.Solve(_profile, Camera, Left, Right, _origin.Origin.Y, dt);
|
||||
if (_needBuild)
|
||||
{
|
||||
_ragdoll.Build(_profile, _solver, Left, Right);
|
||||
_needBuild = false;
|
||||
_prevL = Left.Origin;
|
||||
_prevR = Right.Origin;
|
||||
}
|
||||
Vector3 lv = (Left.Origin - _prevL) / dt, rv = (Right.Origin - _prevR) / dt;
|
||||
_prevL = Left.Origin;
|
||||
_prevR = Right.Origin;
|
||||
_ragdoll.Step(_solver, Left, Right, lv, rv, _origin.Origin.Y, dt);
|
||||
Measure();
|
||||
}
|
||||
|
||||
private void Measure()
|
||||
{
|
||||
foreach (RigidBody3D b in _ragdoll.Bodies)
|
||||
_finite &= BodyMath.IsFinite(b);
|
||||
_minCamZ = Mathf.Min(_minCamZ, Camera.Origin.Z);
|
||||
_minEyeZ = Mathf.Min(_minEyeZ, _ragdoll.Eye.Z);
|
||||
if (_t < _measureFrom)
|
||||
return;
|
||||
_maxEyeErr = Mathf.Max(_maxEyeErr, _ragdoll.Eye.DistanceTo(Camera.Origin));
|
||||
float handErr = Mathf.Max(_ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin));
|
||||
_maxHandErr = Mathf.Max(_maxHandErr, handErr);
|
||||
_endHandErr = handErr;
|
||||
float speed = 0f;
|
||||
foreach (RigidBody3D b in _ragdoll.Bodies)
|
||||
speed = Mathf.Max(speed, b.LinearVelocity.Length());
|
||||
_maxSpeed = speed;
|
||||
for (int side = 0; side < 2; side++)
|
||||
{
|
||||
Vector3 up = _solver.Elbow[side] - _solver.Shoulder[side], fore = _solver.Wrist[side] - _solver.Elbow[side];
|
||||
float target = up.AngleTo(fore);
|
||||
_maxElbowErr = Mathf.Max(_maxElbowErr, Mathf.Abs(target - _ragdoll.ElbowFlex(side)));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private int NewSnaps => _ragdoll.Snaps - _snapsAtStart;
|
||||
|
||||
private static StaticBody3D MakeStatic(Vector3 size, Vector3 pos, uint mask = 0)
|
||||
{
|
||||
StaticBody3D body = new() { Position = pos, CollisionMask = mask };
|
||||
body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
|
||||
return body;
|
||||
}
|
||||
|
||||
private RigidBody3D MakeBox(Vector3 size, float mass, Vector3 pos)
|
||||
{
|
||||
RigidBody3D body = new() { Mass = mass, Position = pos, CollisionMask = 29 };
|
||||
body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
|
||||
_stage.AddChild(body);
|
||||
return body;
|
||||
}
|
||||
|
||||
// Standing, looking ahead, hands in front at waist height (elbows ~90°), controllers pointing forward.
|
||||
private void Rest()
|
||||
{
|
||||
_origin = Transform3D.Identity;
|
||||
_headLocal = new Transform3D(Basis.Identity, new Vector3(0f, EyeHeight, 0f));
|
||||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
|
||||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 1.0f, -0.3f));
|
||||
_solver.Reset();
|
||||
_needBuild = true;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- hinge sign
|
||||
|
||||
private RigidBody3D _hingeBody;
|
||||
private HingeJoint3D _hinge;
|
||||
private Basis _hingeStart;
|
||||
|
||||
private void SetupHinge()
|
||||
{
|
||||
StaticBody3D anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0f, 5f, 0f));
|
||||
_stage.AddChild(anchor);
|
||||
_hingeBody = new RigidBody3D { Position = new Vector3(0f, 5f, 0f), GravityScale = 0f };
|
||||
_hingeBody.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.4f, 0.1f, 0.1f) } });
|
||||
_stage.AddChild(_hingeBody);
|
||||
_hinge = new HingeJoint3D { Position = new Vector3(0f, 5f, 0f), NodeA = anchor.GetPath(), NodeB = _hingeBody.GetPath() };
|
||||
_hinge.SetFlag(HingeJoint3D.Flag.EnableMotor, true);
|
||||
_hinge.SetParam(HingeJoint3D.Param.MotorTargetVelocity, 1f);
|
||||
_hinge.SetParam(HingeJoint3D.Param.MotorMaxImpulse, 50f);
|
||||
_stage.AddChild(_hinge);
|
||||
_hingeStart = _hingeBody.GlobalTransform.Basis;
|
||||
}
|
||||
|
||||
private void TickHinge(float dt) { }
|
||||
|
||||
private bool FinishHinge()
|
||||
{
|
||||
// Joint +Z = world +Z here. Positive motor velocity rotated the body about ±Z?
|
||||
Vector3 x = _hingeBody.GlobalTransform.Basis.X;
|
||||
float about = _hingeStart.X.Cross(x).Z;
|
||||
PlayerRagdoll.HingeSign = about >= 0f ? 1f : -1f;
|
||||
GD.Print($"RAGDOLL: +motor velocity turns the child about {(about >= 0 ? "+" : "-")}Z → HingeSign {PlayerRagdoll.HingeSign}");
|
||||
return Mathf.Abs(about) > 0.05f;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- phases
|
||||
|
||||
private bool FinishStand()
|
||||
{
|
||||
GD.Print($"RAGDOLL: eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3} m, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, end speed {_maxSpeed:F3} m/s, grounded {_ragdoll.Grounded}, snaps {NewSnaps}, pelvis y {_ragdoll.Pelvis.GlobalPosition.Y:F3}");
|
||||
return _finite && NewSnaps == 0 && _ragdoll.Grounded && _maxEyeErr < 0.03f && _maxHandErr < 0.03f && _maxSpeed < 0.05f && _maxElbowErr < Mathf.DegToRad(8f);
|
||||
}
|
||||
|
||||
private readonly List<string> _reachLog = new();
|
||||
private bool _reachOk = true;
|
||||
|
||||
private void TickReach(float dt)
|
||||
{
|
||||
_measureFrom = float.MaxValue;
|
||||
float reach = _profile.ArmLength;
|
||||
float sh = EyeHeight - _profile.ShoulderDrop;
|
||||
(Vector3 l, Vector3 r)[] poses =
|
||||
{
|
||||
(new Vector3(-0.2f, sh, -reach - 0.05f), new Vector3(0.2f, sh, -reach - 0.05f)), // forward, straight
|
||||
(new Vector3(-_profile.ShoulderHalfWidth - reach, sh, 0f), new Vector3(_profile.ShoulderHalfWidth + reach, sh, 0f)), // T
|
||||
(new Vector3(-0.2f, sh + reach, -0.05f), new Vector3(0.2f, sh + reach, -0.05f)), // overhead
|
||||
};
|
||||
int i = Mathf.Min((int)(_t / 1.5f), poses.Length - 1);
|
||||
float local = _t - i * 1.5f;
|
||||
// Controllers point along the arm, as a real hand does at full reach.
|
||||
Vector3 shL = new(-_profile.ShoulderHalfWidth, sh, 0.06f), shR = new(_profile.ShoulderHalfWidth, sh, 0.06f);
|
||||
_leftLocal = new Transform3D(Aim(poses[i].l - shL), poses[i].l);
|
||||
_rightLocal = new Transform3D(Aim(poses[i].r - shR), poses[i].r);
|
||||
if (local > 1.4f && local - dt <= 1.4f)
|
||||
{
|
||||
// Unreachable by up to the girdle travel: the hand stops at the arm's end, not inside the body.
|
||||
float errL = _ragdoll.Hand(0).GlobalPosition.DistanceTo(Left.Origin), errR = _ragdoll.Hand(1).GlobalPosition.DistanceTo(Right.Origin);
|
||||
float e = Mathf.Max(errL, errR);
|
||||
_reachLog.Add($"pose {i}: hand error {e:F3} m");
|
||||
_reachOk &= e < 0.06f;
|
||||
}
|
||||
}
|
||||
|
||||
// Controller basis whose aim (−Z) points along `dir`, thumb up where possible.
|
||||
private static Basis Aim(Vector3 dir)
|
||||
{
|
||||
Vector3 z = -dir.Normalized();
|
||||
Vector3 up = Mathf.Abs(z.Y) > 0.9f ? Vector3.Back : Vector3.Up;
|
||||
Vector3 x = up.Cross(z).Normalized();
|
||||
return new Basis(x, z.Cross(x), z);
|
||||
}
|
||||
|
||||
private bool FinishReach()
|
||||
{
|
||||
GD.Print($"RAGDOLL: {string.Join(", ", _reachLog)}; snaps {NewSnaps}");
|
||||
bool ok = _finite && NewSnaps == 0 && _reachOk && _reachLog.Count == 3;
|
||||
_reachLog.Clear();
|
||||
_reachOk = true;
|
||||
return ok;
|
||||
}
|
||||
|
||||
private void TickSwing(float dt)
|
||||
{
|
||||
float a = Mathf.Sin(_t * Mathf.Tau * 1.5f);
|
||||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.25f, 1.15f + 0.25f * a, -0.35f - 0.15f * a));
|
||||
_rightLocal = new Transform3D(new Basis(Vector3.Right, 0.8f * a), new Vector3(0.25f + 0.2f * a, 1.15f - 0.25f * a, -0.35f));
|
||||
}
|
||||
|
||||
private bool FinishSwing()
|
||||
{
|
||||
GD.Print($"RAGDOLL: hand lag max {_maxHandErr:F3} m (limit 0.15), eye error max {_maxEyeErr:F3}, snaps {NewSnaps}");
|
||||
return _finite && NewSnaps == 0 && _maxHandErr < 0.15f && _maxEyeErr < 0.08f;
|
||||
}
|
||||
|
||||
private const float WallFace = -1.0f;
|
||||
|
||||
private void SetupWall()
|
||||
{
|
||||
Rest();
|
||||
_stage.AddChild(MakeStatic(new Vector3(4f, 3f, 0.4f), new Vector3(0f, 1.5f, WallFace - 0.2f), 29));
|
||||
}
|
||||
|
||||
private void TickRoomWalk(float dt)
|
||||
{
|
||||
Vector3 step = new(0f, 0f, -1f * dt);
|
||||
_headLocal.Origin += step;
|
||||
_leftLocal.Origin += step;
|
||||
_rightLocal.Origin += step;
|
||||
}
|
||||
|
||||
private bool FinishWall()
|
||||
{
|
||||
GD.Print($"RAGDOLL: wall face z {WallFace}: camera min z {_minCamZ:F3}, eye min z {_minEyeZ:F3}, snaps {NewSnaps}");
|
||||
return _finite && NewSnaps == 0 && _minCamZ > WallFace - 0.02f && _minEyeZ > WallFace - 0.02f && _minCamZ < WallFace + 0.5f;
|
||||
}
|
||||
|
||||
private const float BarZ = -0.8f;
|
||||
|
||||
private void SetupBar()
|
||||
{
|
||||
Rest();
|
||||
// A beam across the path whose underside is at 1.2 m.
|
||||
_stage.AddChild(MakeStatic(new Vector3(4f, 0.1f, 0.1f), new Vector3(0f, 1.25f, BarZ), 29));
|
||||
}
|
||||
|
||||
private bool FinishBarBlocked()
|
||||
{
|
||||
// The chest hits the beam; the head may lean over it, but the view can't run off ahead of the body.
|
||||
GD.Print($"RAGDOLL: bar at z {BarZ}: eye min z {_minEyeZ:F3}, camera min z {_minCamZ:F3}, snaps {NewSnaps}");
|
||||
return _finite && NewSnaps == 0 && _minEyeZ > BarZ - 0.25f && _minCamZ > _minEyeZ - PlayerRagdoll.FreeSlack - 0.02f;
|
||||
}
|
||||
|
||||
private void TickCrouchWalk(float dt)
|
||||
{
|
||||
// Crouch over the first second (eyes to 1.0 m, looking slightly down), then walk forward.
|
||||
float k = Mathf.Min(_t / 1f, 1f);
|
||||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.0f, k), 0f));
|
||||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
|
||||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, Mathf.Lerp(1.0f, 0.65f, k), -0.3f));
|
||||
_stickVelocity = _t > 1.2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero;
|
||||
}
|
||||
|
||||
private bool FinishBarPassed()
|
||||
{
|
||||
GD.Print($"RAGDOLL: crouched: camera min z {_minCamZ:F3} (bar at {BarZ}), eye {_ragdoll.Eye.Y:F2} m, snaps {NewSnaps}");
|
||||
return _finite && NewSnaps == 0 && _minCamZ < BarZ - 0.6f;
|
||||
}
|
||||
|
||||
private void SetupTable()
|
||||
{
|
||||
Rest();
|
||||
_stage.AddChild(MakeStatic(new Vector3(1.4f, 0.05f, 0.8f), new Vector3(0f, 0.725f, -0.85f), 29));
|
||||
}
|
||||
|
||||
private void TickLean(float dt)
|
||||
{
|
||||
float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1.5f, 1f));
|
||||
_headLocal = new Transform3D(new Basis(Vector3.Right, -0.6f * k), new Vector3(0f, Mathf.Lerp(EyeHeight, 1.3f, k), Mathf.Lerp(0f, -0.4f, k)));
|
||||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
|
||||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, 0.85f, Mathf.Lerp(-0.3f, -0.7f, k)));
|
||||
_measureFrom = 2f;
|
||||
}
|
||||
|
||||
private bool FinishLean()
|
||||
{
|
||||
GD.Print($"RAGDOLL: eye error after the lean max {_maxEyeErr:F3} m (limit 0.08), hand error {_endHandErr:F3}, origin drift {_origin.Origin.Length():F3}, snaps {NewSnaps}");
|
||||
return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _origin.Origin.Length() < 0.05f;
|
||||
}
|
||||
|
||||
private RigidBody3D _crate;
|
||||
private Vector3 _crateStart;
|
||||
|
||||
private void SetupCrate()
|
||||
{
|
||||
Rest();
|
||||
_crate = MakeBox(new Vector3(0.4f, 1.4f, 0.4f), 10f, new Vector3(0f, 0.7f, -0.55f)); // within reach at chest height
|
||||
_crateStart = _crate.Position;
|
||||
}
|
||||
|
||||
private void TickPush(float dt)
|
||||
{
|
||||
float k = Mathf.Clamp((_t - 0.5f) / 2f, 0f, 1f);
|
||||
_leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
|
||||
_rightLocal = new Transform3D(Basis.Identity, new Vector3(0.15f, 1.25f, Mathf.Lerp(-0.2f, -0.6f, k)));
|
||||
}
|
||||
|
||||
private bool FinishCrate()
|
||||
{
|
||||
float moved = _crateStart.Z - _crate.GlobalPosition.Z;
|
||||
GD.Print($"RAGDOLL: crate pushed {moved:F3} m (front face now z {_crate.GlobalPosition.Z + 0.2f:F3}), hands z {_ragdoll.Hand(0).GlobalPosition.Z:F3}/{_ragdoll.Hand(1).GlobalPosition.Z:F3} y {_ragdoll.Hand(0).GlobalPosition.Y:F3}, elbows {Mathf.RadToDeg(_ragdoll.ElbowFlex(0)):F0}°, pelvis z {_ragdoll.Pelvis.GlobalPosition.Z:F3}, snaps {NewSnaps}");
|
||||
return _finite && NewSnaps == 0 && moved > 0.15f;
|
||||
}
|
||||
|
||||
private RigidBody3D _weight;
|
||||
private float _closeSag = -1f;
|
||||
|
||||
private void SetupWeight(bool close)
|
||||
{
|
||||
Rest();
|
||||
float sh = EyeHeight - _profile.ShoulderDrop;
|
||||
_rightLocal = close
|
||||
? new Transform3D(Basis.Identity, new Vector3(0.18f, sh - 0.1f, -0.25f))
|
||||
: new Transform3D(Basis.Identity, new Vector3(_profile.ShoulderHalfWidth, sh, -_profile.ArmLength * 0.95f));
|
||||
_measureFrom = 2f;
|
||||
// Build now so the weight can be welded to the placed hand.
|
||||
Drive(1f / 90f);
|
||||
RigidBody3D hand = _ragdoll.Hand(1);
|
||||
Vector3 at = hand.GlobalTransform * new Vector3(0f, 0f, -0.12f); // just beyond the fingers
|
||||
_weight = MakeBox(new Vector3(0.12f, 0.12f, 0.12f), 12f, at);
|
||||
_weight.GlobalPosition = at;
|
||||
_ragdoll.SetHeld(1, _weight);
|
||||
ConeTwistJoint3D weld = new() { Position = hand.GlobalPosition, NodeA = hand.GetPath(), NodeB = _weight.GetPath(), SwingSpan = 0f, TwistSpan = 0f, Bias = 0f };
|
||||
_stage.AddChild(weld);
|
||||
}
|
||||
|
||||
private bool FinishWeight(bool close)
|
||||
{
|
||||
float sag = Right.Origin.Y - _ragdoll.Hand(1).GlobalPosition.Y;
|
||||
GD.Print($"RAGDOLL: right hand sag {sag:F3} m under 12 kg ({(close ? "close" : "far")}), snaps {NewSnaps}");
|
||||
if (close)
|
||||
{
|
||||
_closeSag = sag;
|
||||
return _finite && NewSnaps == 0 && sag < 0.08f;
|
||||
}
|
||||
return _finite && NewSnaps == 0 && sag > _closeSag + 0.03f;
|
||||
}
|
||||
|
||||
private bool _teleported, _turned;
|
||||
private float _teleportEyeErr;
|
||||
|
||||
private void TickTeleport(float dt)
|
||||
{
|
||||
_measureFrom = 1.2f;
|
||||
if (!_teleported && _t > 0.5f)
|
||||
{
|
||||
Transform3D before = _origin;
|
||||
_origin = new Transform3D(new Basis(Vector3.Up, Mathf.Pi / 2f), new Vector3(5f, 0f, 3f));
|
||||
Transform3D delta = _origin * before.AffineInverse();
|
||||
_ragdoll.TeleportBy(delta);
|
||||
_solver.Shift(delta);
|
||||
_prevL = delta * _prevL;
|
||||
_prevR = delta * _prevR;
|
||||
_teleported = true;
|
||||
}
|
||||
if (!_turned && _t > 0.9f)
|
||||
{
|
||||
Vector3 pivot = Camera.Origin;
|
||||
Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, Mathf.DegToRad(30f)), Vector3.Zero) * new Transform3D(Basis.Identity, -pivot);
|
||||
_origin = rot * _origin;
|
||||
_ragdoll.TeleportBy(rot);
|
||||
_solver.Shift(rot);
|
||||
_prevL = rot * _prevL;
|
||||
_prevR = rot * _prevR;
|
||||
_turned = true;
|
||||
}
|
||||
}
|
||||
|
||||
private bool FinishTeleport()
|
||||
{
|
||||
GD.Print($"RAGDOLL: after teleport + snap turn: eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3}, snaps {NewSnaps}");
|
||||
_teleported = _turned = false;
|
||||
return _finite && NewSnaps == 0 && _maxEyeErr < 0.05f && _maxHandErr < 0.06f;
|
||||
}
|
||||
|
||||
private readonly RandomNumberGenerator _rng = new() { Seed = 11 };
|
||||
private Vector3 _ra, _rb, _rc;
|
||||
|
||||
private void TickRandom(float dt)
|
||||
{
|
||||
if (_t < dt * 1.5f)
|
||||
{
|
||||
_ra = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
|
||||
_rb = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
|
||||
_rc = new Vector3(_rng.Randf(), _rng.Randf(), _rng.Randf()) * Mathf.Tau;
|
||||
}
|
||||
Vector3 W(Vector3 ph, float f) => new(Mathf.Sin(_t * f + ph.X), Mathf.Sin(_t * f * 1.3f + ph.Y), Mathf.Sin(_t * f * 0.7f + ph.Z));
|
||||
Vector3 h = W(_ra, 1.1f);
|
||||
_headLocal = new Transform3D(Basis.FromEuler(new Vector3(0.4f * h.X, 0.9f * h.Y, 0.2f * h.Z)), new Vector3(0.1f * h.Z, EyeHeight - 0.15f + 0.1f * h.X, 0.1f * h.Y));
|
||||
float sh = EyeHeight - _profile.ShoulderDrop;
|
||||
Vector3 l = W(_rb, 2.3f), r = W(_rc, 2.0f);
|
||||
_leftLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * l.Y, 0.8f * l.Z, 1.0f * l.X)), new Vector3(-0.25f + 0.15f * l.X, sh - 0.25f + 0.2f * l.Y, -0.3f + 0.15f * l.Z));
|
||||
_rightLocal = new Transform3D(Basis.FromEuler(new Vector3(1.2f * r.Y, 0.8f * r.Z, 1.0f * r.X)), new Vector3(0.25f + 0.15f * r.X, sh - 0.25f + 0.2f * r.Y, -0.3f + 0.15f * r.Z));
|
||||
}
|
||||
|
||||
private bool FinishSoak()
|
||||
{
|
||||
GD.Print($"RAGDOLL: random motion: eye error max {_maxEyeErr:F3}, hand error max {_maxHandErr:F3}, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, snaps {NewSnaps}");
|
||||
return _finite && NewSnaps == 0 && _maxEyeErr < 0.08f && _maxHandErr < 0.12f;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
uid://bpdb6t6xgwt24
|
||||
+25
-1
@@ -26,8 +26,13 @@ public sealed class TrueBodySolver
|
||||
public const float WristOffset = 0.06f;
|
||||
|
||||
public Vector3 Eye, NeckPivot, NeckBase, UpperChest, Pelvis;
|
||||
public Vector3 PelvisTarget; // un-lagged pelvis (the ragdoll's ground control adds its own lag)
|
||||
public Basis ChestBasis = Basis.Identity;
|
||||
public Basis PelvisBasis = Basis.Identity;
|
||||
public Basis YawBasis = Basis.Identity;
|
||||
public Basis HeadBasis = Basis.Identity;
|
||||
public float Lean { get; private set; }
|
||||
public readonly Vector3[] Pole = new Vector3[2]; // per arm, see ArmSolver.Pole
|
||||
public readonly Vector3[] Shoulder = new Vector3[2];
|
||||
public readonly Vector3[] Elbow = new Vector3[2];
|
||||
public readonly Vector3[] Wrist = new Vector3[2];
|
||||
@@ -40,6 +45,18 @@ public sealed class TrueBodySolver
|
||||
private Vector3 _pelvisVelocity;
|
||||
private float _yaw;
|
||||
|
||||
// A discrete move of the whole player (teleport, snap turn): carry the lagging state along
|
||||
// instead of letting it swing across the gap.
|
||||
public void Shift(Transform3D delta)
|
||||
{
|
||||
if (!Initialized)
|
||||
return;
|
||||
Pelvis = delta * Pelvis;
|
||||
_pelvisVelocity = delta.Basis * _pelvisVelocity;
|
||||
Vector3 f = delta.Basis * new Basis(Vector3.Up, _yaw) * Vector3.Forward;
|
||||
_yaw = Mathf.Atan2(-f.X, -f.Z);
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
Initialized = false;
|
||||
@@ -52,6 +69,7 @@ public sealed class TrueBodySolver
|
||||
{
|
||||
Eye = head.Origin;
|
||||
Basis headBasis = head.Basis.Orthonormalized();
|
||||
HeadBasis = headBasis;
|
||||
NeckPivot = Eye + headBasis * EyeToNeckPivot;
|
||||
WristTarget[0] = left * new Vector3(0f, 0f, WristOffset);
|
||||
WristTarget[1] = right * new Vector3(0f, 0f, WristOffset);
|
||||
@@ -73,6 +91,7 @@ public sealed class TrueBodySolver
|
||||
_yaw = hmdYaw;
|
||||
_yaw = Mathf.LerpAngle(_yaw, targetYaw, 1f - Mathf.Exp(-dt * YawSmoothRate));
|
||||
Basis yawBasis = new Basis(Vector3.Up, _yaw);
|
||||
YawBasis = yawBasis;
|
||||
|
||||
// Crouch lean: measured at the NECK PIVOT (nodding moves the eyes ~10 cm but not the pivot),
|
||||
// below its standing height the chest pitches forward; looking down adds a little.
|
||||
@@ -81,6 +100,7 @@ public sealed class TrueBodySolver
|
||||
Crouch = Mathf.Clamp((standingPivot - pivotAboveFloor) / (CrouchRange * p.EyeHeight), 0f, 1f);
|
||||
float headPitchDown = Mathf.Max(0f, -Mathf.Asin(Mathf.Clamp(headFwd.Y, -1f, 1f)));
|
||||
float lean = Crouch * ChestLeanMax + headPitchDown * HeadPitchToChest;
|
||||
Lean = lean;
|
||||
ChestBasis = yawBasis * new Basis(Vector3.Right, -lean);
|
||||
|
||||
// Neck: from the head pivot down along the average of chest-up and head-up, sized so a
|
||||
@@ -96,6 +116,7 @@ public sealed class TrueBodySolver
|
||||
// Pelvis: hangs below the upper chest along the trunk; follows through a critically damped
|
||||
// spring so it lags quick head motion (hip sway) instead of being rigidly attached.
|
||||
Vector3 pelvisTarget = UpperChest - ChestBasis.Y * (TrunkRatio * p.Stature);
|
||||
PelvisTarget = pelvisTarget;
|
||||
if (!Initialized)
|
||||
Pelvis = pelvisTarget;
|
||||
float w = Mathf.Tau * PelvisFrequency;
|
||||
@@ -138,6 +159,7 @@ public sealed class TrueBodySolver
|
||||
|
||||
_arms[side].Solve(Shoulder[side], WristTarget[side], ChestBasis, outward, handBasis, p.UpperArm, p.Forearm, dt);
|
||||
Elbow[side] = _arms[side].Elbow;
|
||||
Pole[side] = _arms[side].Pole;
|
||||
Wrist[side] = _arms[side].Wrist;
|
||||
}
|
||||
}
|
||||
@@ -157,6 +179,8 @@ public partial class TrueBody : Node3D
|
||||
private readonly System.Collections.Generic.List<MeshInstance3D> _joints = new();
|
||||
|
||||
public TrueBodySolver Solver { get; } = new();
|
||||
// Set by PlayerRig, which solves at the right point of its own tick (after moving the origin).
|
||||
public bool ExternallySolved;
|
||||
public bool HasPose => Solver.Initialized;
|
||||
|
||||
public override void _Ready()
|
||||
@@ -178,7 +202,7 @@ public partial class TrueBody : Node3D
|
||||
public override void _PhysicsProcess(double delta)
|
||||
{
|
||||
BodyProfile profile = BodyProfile.Current;
|
||||
if (profile == null)
|
||||
if (profile == null || ExternallySolved)
|
||||
return;
|
||||
Solver.Solve(profile, _camera.GlobalTransform, _left.GlobalTransform, _right.GlobalTransform, _origin.GlobalPosition.Y, (float)delta);
|
||||
}
|
||||
|
||||
@@ -79,6 +79,16 @@ public partial class WorldPanel : Node3D
|
||||
SetPanelActive(false);
|
||||
}
|
||||
|
||||
// This controller currently points at (or touches) the menu — its stick scrolls the menu,
|
||||
// so locomotion must ignore it.
|
||||
public static bool IsPointing(XRController3D controller)
|
||||
{
|
||||
if (_instance == null || !_instance.Visible)
|
||||
return false;
|
||||
int hand = controller == _instance._left ? 0 : controller == _instance._right ? 1 : -1;
|
||||
return hand >= 0 && _instance._screen.GetMode(hand) != MenuScreen.PointerMode.None;
|
||||
}
|
||||
|
||||
public static bool IsBlocking(XRController3D controller) =>
|
||||
_instance != null && _instance.Visible &&
|
||||
_instance.GlobalPosition.DistanceTo(controller.GlobalPosition) < DirectGrabRadius;
|
||||
|
||||
Reference in new issue
Block a user