FoxyzandClaude Opus 5.5
2aff2b61da
- Step rule + lower-body collider: no walking onto tables, steps <= 0.35 m - Structural hand strength: two-hand mantle/pull-up works, one hand can't - Arms aim from the physical shoulder horizontally, keep True Body pose vertically - Crawl pose, crawl carry, 35 deg slope limit, ground feed-forward ignores play-space corrections (fixes a runaway feedback loop) - Grab static world (climbing), procedural visual legs - Playground sandbox; menu strength slider, spectator (old drag) and turn toggles - Physics 144 Hz with interpolation; legacy chase-body spikes pin 90 Hz - RagdollSpike: 27 phases incl. mantle, pull-up, crawl, ledge drop, ramps Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
869 lines
48 KiB
C#
869 lines
48 KiB
C#
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;
|
||
// STRUCTURAL strength: force along the arm (straight arm pressing on a ledge, hanging from a
|
||
// bar, carrying a bucket at your side) goes through bone and is strong; force across the arm
|
||
// needs muscle torque and stays at HandGuideForce. Cap = lerp(base, structural, alignment).
|
||
// Two arms ≈ 1.2 × body weight → two hands mantle/pull up, one can't (Nimso).
|
||
public const float HandStructuralForce = 400f;
|
||
public const float StructuralDeadband = 0.03f;
|
||
// Nimso's one global strength value: scales every muscle and the hand guides (menu slider).
|
||
public static float Strength = 1f;
|
||
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
|
||
public const float MaxStepHeight = 0.35f;
|
||
public const float MaxSlopeDegrees = 35f; // steeper ground gives no support (Nimso: "slopes beyond an angle can't be run up") // higher ground under the hips isn't stood on unless the body got up there
|
||
public const float BracedAirControl = 150f; // N of horizontal hip carry while airborne but braced on the hands (mantling)
|
||
public const float CrawlCarry = 300f; // N: the torso lies on the floor in a crawl; knees/hands would push (not simulated)
|
||
public const float TorsoFriction = 0.3f; // clothing on floor: lets a crawl slide
|
||
// Lower-body collider: a box from the hips down to MaxStepHeight above the feet, so knee-high
|
||
// obstacles block walking (no walking over tables) while lower steps pass under it. It slides
|
||
// up into the torso when crouching and switches off when crawling (Nimso: legs don't snag).
|
||
public const float LegsMass = 6f;
|
||
public const float LegsKp = 30f, LegsForce = 3000f, LegsTorque = 400f;
|
||
public const float LegsOffAbove = 0.25f; // collider disabled once slid this far above the hips
|
||
public const float MantleLift = 0.08f; // braced + hips this far above the support goal = being lifted (legs tuck)
|
||
// 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 AirSlack = 0.05f; // climbing/hanging (braced): the view rides the body tightly
|
||
public const float FallSlack = 0.25f; // free airborne: brief ground loss (sliding off a steep slope) mustn't sink the view
|
||
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 float PrevFlexTarget = -1f;
|
||
public float GuideCap = HandGuideForce;
|
||
public PhysicsBody3D Held;
|
||
}
|
||
|
||
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 => _footY;
|
||
public bool Braced { get; private set; } // a hand touches or holds static world
|
||
// Really climbing: a hand holds static world, or the arms are lifting the body (mantle).
|
||
public bool Climbing => Built && (_arms[0].Held is StaticBody3D || _arms[1].Held is StaticBody3D || _lifted);
|
||
public RigidBody3D Legs { 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 float _footY;
|
||
private float _supportGoalY;
|
||
private bool _lifted;
|
||
private float _legsHeight;
|
||
private Drive _legsDrive;
|
||
private bool _legsSolid = true;
|
||
private ProceduralLegs _legVisuals;
|
||
private float _appliedStrength = 1f;
|
||
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 - LegsMass, BodyLayer, WorldMask, pants,
|
||
c * new Transform3D(Basis.Identity, hip + _pelvisCenterLocal));
|
||
float standingHip = pose.PelvisTarget.Y - (pose.Eye.Y - p.EyeHeight);
|
||
_legsHeight = Mathf.Max(0.2f, standingHip - MaxStepHeight);
|
||
Legs = MakeBody("Legs", new BoxShape3D { Size = new Vector3(0.28f * s, _legsHeight, 0.2f * s) }, LegsMass, BodyLayer, WorldMask, pants,
|
||
c * new Transform3D(Basis.Identity, hip - new Vector3(0f, _legsHeight * 0.5f, 0f)), visible: false);
|
||
_footY = pose.Eye.Y - p.EyeHeight;
|
||
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);
|
||
foreach (RigidBody3D torso in new[] { Pelvis, Chest, Head, Legs })
|
||
torso.PhysicsMaterialOverride = new PhysicsMaterial { Friction = TorsoFriction };
|
||
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);
|
||
_legsDrive = MakeDrive(Pelvis, Legs, c * new Transform3D(Basis.Identity, hip), LegsForce, LegsTorque);
|
||
_legVisuals = new ProceduralLegs { Name = "LegVisuals" };
|
||
AddChild(_legVisuals);
|
||
_legVisuals.Setup(Pelvis, -_pelvisCenterLocal, p.Stature, pants, new Color(0.15f, 0.15f, 0.17f));
|
||
_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;
|
||
_appliedStrength = 1f;
|
||
_legsSolid = true;
|
||
ApplyStrength();
|
||
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, bool visible = true)
|
||
{
|
||
RigidBody3D body = new() { Name = name, Mass = mass, CollisionLayer = layer, CollisionMask = mask, Transform = at, ContactMonitor = true, MaxContactsReported = 4 };
|
||
body.AddChild(new CollisionShape3D { Shape = shape });
|
||
if (shape is BoxShape3D box && layer != 0 && visible)
|
||
{
|
||
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, bool physicalArms = false, float dt = 0f)
|
||
{
|
||
Basis chest = s.ChestBasis;
|
||
// Hips take half of a crouch lean, all of it in a crawl (keeps them off the floor).
|
||
float share = Mathf.Lerp(PelvisLeanShare, 1f, Mathf.Clamp((s.Lean - TrueBodySolver.ChestLeanMax) / 0.5f, 0f, 1f));
|
||
Basis pelvis = s.YawBasis * new Basis(Vector3.Right, -s.Lean * share);
|
||
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);
|
||
_targets[Legs] = new Transform3D(s.YawBasis, hip - Vector3.Up * (_legsHeight * 0.5f)); // snap pose only; LegsDrive steps it
|
||
// 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();
|
||
|
||
if (physicalArms)
|
||
{
|
||
// Re-aim the arm from the PHYSICAL shoulder. Horizontally the body may be held away from
|
||
// the camera (pressed against a ledge, leaning over a table): the arm reaches for the
|
||
// real hand. Vertically a sagging body keeps its arm pose (the aim point moves with
|
||
// the shoulder), so straight real arms stay straight virtual arms — pillars that hold
|
||
// you up when crawling or press you up a ledge (the hand guide adds the rest).
|
||
Transform3D chestPhys = Chest.GlobalTransform.Orthonormalized();
|
||
Vector3 shPhys = chestPhys * (a.Shoulder.AnchorA + _girdle[side]);
|
||
Transform3D ctrl = _targets[a.Hand];
|
||
Vector3 aim = ctrl * new Vector3(0f, 0f, WristOffset) + Vector3.Up * (shPhys.Y - sh.Y);
|
||
ArmSolver solver = _armSolvers[side];
|
||
solver.Solve(shPhys, aim, chestPhys.Basis, chestPhys.Basis.X * (side == 0 ? -1f : 1f),
|
||
ctrl.Basis, a.UpperLen, a.ForeLen, dt);
|
||
Vector3 d = solver.Wrist - shPhys;
|
||
Vector3 h = d.LengthSquared() > 1e-8f ? solver.Pole.Cross(d.Normalized()) : chestPhys.Basis.X;
|
||
h = h.LengthSquared() > 1e-8f ? h.Normalized() : chestPhys.Basis.X;
|
||
Vector3 u = (solver.Elbow - shPhys).Normalized(), f = (solver.Wrist - solver.Elbow).Normalized();
|
||
_targets[a.Upper] = new Transform3D(BoneBasis(u, h), shPhys + u * (a.UpperLen * 0.5f));
|
||
_targets[a.Fore] = new Transform3D(BoneBasis(f, h), solver.Elbow + f * (a.ForeLen * 0.5f));
|
||
}
|
||
}
|
||
}
|
||
|
||
private readonly ArmSolver[] _armSolvers = { new(), new() };
|
||
|
||
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, physicalArms: true, dt);
|
||
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);
|
||
if (Strength != _appliedStrength)
|
||
ApplyStrength();
|
||
Braced = false;
|
||
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);
|
||
Braced |= IsBraced(a);
|
||
}
|
||
GroundControl(pelvisTarget, floorY, dt);
|
||
LegsDrive(s);
|
||
_legVisuals.Tick(_footY, Grounded, !_legsSolid, s.YawBasis, 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);
|
||
}
|
||
|
||
private void ApplyStrength()
|
||
{
|
||
_appliedStrength = Strength;
|
||
float k = Mathf.Max(0.05f, Strength);
|
||
SetAngularCap(_spine.Joint, SpineTorque * k);
|
||
SetAngularCap(_neck.Joint, NeckTorque * k);
|
||
foreach (Arm a in _arms)
|
||
{
|
||
SetAngularCap(a.Shoulder.Joint, ShoulderTorque * k);
|
||
SetAngularCap(a.Guide.Joint, HandGuideTorque * k);
|
||
a.Elbow.SetParam(HingeJoint3D.Param.MotorMaxImpulse, ElbowTorque * k);
|
||
}
|
||
}
|
||
|
||
private static void SetAngularCap(Generic6DofJoint3D j, float cap)
|
||
{
|
||
j.SetParamX(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
|
||
j.SetParamY(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
|
||
j.SetParamZ(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
|
||
}
|
||
|
||
private static void SetLinearCap(Generic6DofJoint3D j, float cap)
|
||
{
|
||
j.SetParamX(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap);
|
||
j.SetParamY(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap);
|
||
j.SetParamZ(Generic6DofJoint3D.Param.LinearMotorForceLimit, cap);
|
||
}
|
||
|
||
// A hand pressing on or holding static world (a ledge, a wall, a grabbed bar).
|
||
private static bool IsBraced(Arm a)
|
||
{
|
||
if (a.Held is StaticBody3D)
|
||
return true;
|
||
foreach (Node3D n in a.Hand.GetCollidingBodies())
|
||
if (n is StaticBody3D)
|
||
return true;
|
||
return false;
|
||
}
|
||
|
||
// The lower-body collider hangs from the actual hips, its bottom never below a step above the
|
||
// feet; once it would have to slide far up into the torso (deep crouch, crawl) it stops colliding.
|
||
private void LegsDrive(TrueBodySolver s)
|
||
{
|
||
Transform3D pelvis = Pelvis.GlobalTransform;
|
||
Vector3 hip = pelvis * (-_pelvisCenterLocal);
|
||
float minCenter = _footY + MaxStepHeight + _legsHeight * 0.5f;
|
||
float center = Mathf.Max(hip.Y - _legsHeight * 0.5f, minCenter);
|
||
// Tucked: deep crouch/crawl, or being lifted by the arms (mantling — the knee comes up).
|
||
_lifted = Braced && Pelvis.GlobalPosition.Y > _supportGoalY + MantleLift;
|
||
bool solid = center + _legsHeight * 0.5f < hip.Y + LegsOffAbove && !_lifted;
|
||
if (solid != _legsSolid)
|
||
{
|
||
_legsSolid = solid;
|
||
// The shape itself is disabled: Box3D keeps existing contacts when only the layer changes.
|
||
Legs.GetChild<CollisionShape3D>(0).SetDeferred(CollisionShape3D.PropertyName.Disabled, !solid);
|
||
}
|
||
if (!solid)
|
||
center = hip.Y; // tucked into the pelvis, out of the way
|
||
Vector3 desired = new(hip.X, center, hip.Z);
|
||
Quaternion frameA = Rot(Pelvis) * _legsDrive.FrameA;
|
||
SetLinearVelocity(_legsDrive.Joint, frameA.Inverse() * ((desired - Legs.GlobalPosition) * LegsKp));
|
||
Quaternion target = (Rot(Pelvis) * _legsDrive.FrameA).Inverse() * (s.YawBasis.GetRotationQuaternion() * _legsDrive.FrameB);
|
||
Quaternion current = frameA.Inverse() * (Rot(Legs) * _legsDrive.FrameB);
|
||
SetAngularVelocity(_legsDrive.Joint, (BodyMath.RotationError(target, current) * LegsKp).LimitLength(MuscleMaxSpin));
|
||
}
|
||
|
||
// 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);
|
||
// Arm targets are solved from the physical chest, so the physical chest is the parent frame.
|
||
Quaternion target = (chest.Basis.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 err = target.Origin - a.Hand.GlobalPosition;
|
||
Vector3 desired = (targetVelocity + err * HandGuideKp).LimitLength(HandGuideMaxSpeed);
|
||
Quaternion frameA = Rot(Chest) * a.Guide.FrameA;
|
||
Vector3 shoulder = a.Upper.GlobalTransform * a.Shoulder.AnchorB;
|
||
Vector3 axis = a.Hand.GlobalPosition - shoulder;
|
||
// Axial share of the error, with a 3 cm floor so a nearly-reached target's noisy error
|
||
// direction can't flip a held load into "structural".
|
||
float align = axis.LengthSquared() > 1e-8f ? Mathf.Abs(err.Dot(axis.Normalized())) / (err.Length() + StructuralDeadband) : 0f;
|
||
float cap = Mathf.Lerp(HandGuideForce, HandStructuralForce, align) * Mathf.Max(0.05f, Strength);
|
||
if (Mathf.Abs(cap - a.GuideCap) > 1f)
|
||
{
|
||
a.GuideCap = cap;
|
||
SetLinearCap(a.Guide.Joint, cap);
|
||
}
|
||
SetLinearVelocity(a.Guide.Joint, frameA.Inverse() * (desired - anchorVelocity));
|
||
// Angular: hand rotation onto the controller rotation, as a relative spin vs the chest.
|
||
Vector3 rotErr = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(a.Hand));
|
||
Vector3 wWorld = (rotErr * 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);
|
||
bool wasGrounded = Grounded;
|
||
Grounded = false;
|
||
if (hit.Count > 0)
|
||
{
|
||
Vector3 point = (Vector3)hit["position"];
|
||
Vector3 normal = (Vector3)hit["normal"];
|
||
bool inReach = pos.Y - point.Y < hipAboveFloor + GroundProbeExtra;
|
||
bool steep = normal.Y <= Mathf.Cos(Mathf.DegToRad(MaxSlopeDegrees));
|
||
if (inReach && steep && wasGrounded && point.Y <= _footY + MaxStepHeight)
|
||
{
|
||
// The foot of a too-steep slope: keep standing on the ground we're on (the legs
|
||
// collider stops us); dropping support here would collapse the body.
|
||
Grounded = true;
|
||
}
|
||
else if (inReach && !steep)
|
||
{
|
||
// Step rule: ground more than a step above the feet is only stood on once the
|
||
// hips are already well above it (climbed/mantled/landed there) — otherwise it's an
|
||
// obstacle the legs collider holds us back from, and the feet stay where they were.
|
||
// Higher ground climbed onto must be under the hips' back edge too (not just a
|
||
// pelvis centre hanging over the lip).
|
||
bool clear = point.Y <= _footY + MaxStepHeight || GroundBehindAt(pos, point.Y);
|
||
bool reachable = point.Y <= _footY + MaxStepHeight
|
||
|| ((!wasGrounded || !_legsSolid || pos.Y - point.Y > 0.6f * hipAboveFloor) && clear);
|
||
Grounded = true;
|
||
if (reachable)
|
||
_footY = point.Y;
|
||
}
|
||
}
|
||
|
||
// Feed-forward from the GOAL (pose + ground offset): when the play-space floor follows the
|
||
// ground, the pose target and the ground offset move by the same amount in opposite
|
||
// directions — differentiating the pose alone would launch the body.
|
||
Vector3 goalNow = target.Origin + Vector3.Up * (_footY - floorY);
|
||
Vector3 targetVelocity = _havePelvisPrev && dt > 0f ? (goalNow - _pelvisTargetPrev) / dt : Vector3.Zero;
|
||
_pelvisTargetPrev = goalNow;
|
||
_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);
|
||
_supportGoalY = goal.Y;
|
||
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);
|
||
float carry = Mathf.Max(GroundFriction * support, _legsSolid ? 0f : CrawlCarry);
|
||
Vector3 horizontal = new Vector3(force.X, 0f, force.Z).LimitLength(carry);
|
||
Pelvis.ApplyCentralForce(horizontal + Vector3.Up * support);
|
||
}
|
||
else if (Braced)
|
||
{
|
||
// Hanging/mantling on the hands: the hips still drift after the head a little, so you can
|
||
// shift your weight over a ledge you're pressing yourself up onto.
|
||
Vector3 lead = target.Origin - pos;
|
||
lead.Y = 0f;
|
||
Vector3 v = Pelvis.LinearVelocity;
|
||
v.Y = 0f;
|
||
Pelvis.ApplyCentralForce(((lead * 20f - v * 8f) * BodyMass).LimitLength(BracedAirControl));
|
||
}
|
||
|
||
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);
|
||
// Only a climbing body drags the view vertically through geometry; otherwise a
|
||
// camera-to-eye line grazing a slope would sink the view (and with it the pose).
|
||
if (!Climbing)
|
||
correction.Y = 0f;
|
||
}
|
||
else
|
||
{
|
||
float slack = Grounded ? FreeSlack : Climbing ? AirSlack : FallSlack;
|
||
if (lead.Length() > slack)
|
||
correction = -lead * (1f - slack / lead.Length());
|
||
}
|
||
}
|
||
if (Grounded)
|
||
{
|
||
correction.Y = 0f;
|
||
correction.Y += (GroundY - floorY) * (1f - Mathf.Exp(-dt * GroundFollowRate));
|
||
}
|
||
// The pose target moves with the play space: a correction is not intended motion, so it
|
||
// must not show up in ground control's feed-forward (that loop ran away at 7 m/s).
|
||
_pelvisTargetPrev += correction;
|
||
return correction;
|
||
}
|
||
|
||
private PhysicsShapeQueryParameters3D _cameraQuery;
|
||
|
||
// Second ground ray 12 cm behind the hips (toward the body's back): ground at least `groundY`.
|
||
private bool GroundBehindAt(Vector3 pelvisPos, float groundY)
|
||
{
|
||
Vector3 back = Pelvis.GlobalTransform.Basis.Z;
|
||
back.Y = 0f;
|
||
back = back.LengthSquared() > 1e-6f ? back.Normalized() * 0.12f : Vector3.Zero;
|
||
var query = PhysicsRayQueryParameters3D.Create(pelvisPos + back, pelvisPos + back + Vector3.Down * 3f, WorldMask);
|
||
Godot.Collections.Dictionary hit = GetWorld3D().DirectSpaceState.IntersectRay(query);
|
||
return hit.Count > 0 && ((Vector3)hit["position"]).Y > groundY - 0.05f;
|
||
}
|
||
|
||
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;
|
||
_footY += delta.Origin.Y;
|
||
foreach (RigidBody3D b in _bodies)
|
||
b.ResetPhysicsInterpolation();
|
||
}
|
||
|
||
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;
|
||
foreach (RigidBody3D b in _bodies)
|
||
b.ResetPhysicsInterpolation();
|
||
}
|
||
|
||
// 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, PhysicsBody3D 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);
|
||
}
|
||
}
|
||
}
|