FoxyzandClaude Opus 5.5
e821728fba
- Look-down flip: body facing from BodyMath.HeadFacing (visor + crown), robust past vertical; stick direction uses it too; crouch lean pitch measured against the facing. - Landing: falling legs touch down only within their reach, so leg strength stops the body below its stance (dip scales with impact, big falls fold the legs). Rise eased so gravity can stop it at the stance; legs untuck speed-limited. Fixed the vertical play-space correction leaking into ground control's feed-forward (a bounce). - Jump on right A: countermovement dip + leg push over the stroke (crouch = higher). - Stick crouch on right stick Y (RagdollDriver.StickCrouch: virtual floor offset). - Menu summon on left X only. - XR origin drawn with the bodies' physics interpolation (camera/body desync). - RagdollSpike: landing/fall/jump/look-down/stick-crouch phases, `-- only=` filter. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
1109 lines
57 KiB
C#
1109 lines
57 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;
|
||
// Asymmetric by direction (Nimso; human strength tables): pulling the arm DOWN (lats/back —
|
||
// pull-ups, hanging, climbing) is roughly twice as strong as raising it (front delts — a weight
|
||
// held out front), and elbow flexion (biceps) beats extension.
|
||
public const float ShoulderKp = 20f, ShoulderTorque = 35f, ShoulderPullTorque = 100f;
|
||
public const float ElbowKp = 20f, ElbowTorque = 45f, ElbowFlexTorque = 70f;
|
||
public const float MuscleMaxSpin = 15f;
|
||
// Feed-forwards are derivatives of tracking: low-pass them or tracking jitter (±1–2 mm per
|
||
// sample) becomes hundreds of newtons of buzz (RagdollSpike REAL input mode).
|
||
public const float MuscleFeedForwardTau = 0.05f; // s
|
||
public const float GroundFeedForwardTau = 0.12f; // s
|
||
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 = 4.5f; // max support, × body weight (real legs brake a landing at 3–5 g; 2× sank ~0.4 m)
|
||
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 MaxRiseSpeed = 1.2f; // m/s the legs lift the body toward its stance
|
||
// Landing (owner 2.6: the sink is feedback, it just had to be followed by the view): falling legs
|
||
// only touch down when the hips are within their real reach of the ground. Before, support
|
||
// began GroundProbeExtra early and braked the fall in the air — no dip at all. Now LegStrength
|
||
// stops the body BELOW its stance, so the dip scales with the impact: ~0.13 m from a hop,
|
||
// ~0.3 m from 1 m, legs fully folded from a big fall (Boneworks) — then the normal rise.
|
||
public const float LandingReach = 0.05f; // m past the pose's hip height
|
||
public const float AbsorbTime = 0.4f; // s after a hard touchdown the head guide stays off (it would hold the dip up)
|
||
// Jump (right A): a short countermovement dip, then the legs push JumpForce over the stroke up to
|
||
// full extension. Takeoff speed is physics — v² = 2·(F/m − g)·stroke — so a crouch before
|
||
// the press (real or stick) jumps higher: ~0.45 m standing, ~0.7 m from a deep crouch.
|
||
public const float JumpForce = 2.4f; // × body weight
|
||
public const float JumpDip = 0.2f; // m the hips drop first when pressed from standing
|
||
public const float JumpExtension = 0.06f; // m above the standing hips at toe-off
|
||
public const float JumpMaxStroke = 0.5f;
|
||
public const float JumpBuffer = 0.15f; // s a press is remembered before touchdown
|
||
public const float CoyoteTime = 0.12f; // s after leaving the ground a jump still works
|
||
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;
|
||
public const float BlockedPush = 150f; // N horizontal leg push while the hips are held back // 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
|
||
public const float LimbFriction = 0.25f;
|
||
// 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, LegsMaxSpeed = 1.5f;
|
||
public const float LegsOffAbove = 0.25f; // collider disabled once slid this far above the hips
|
||
public const float MantleLift = 0.03f; // braced + hips this far above the support goal = being lifted (legs tuck)
|
||
public const float UntuckDelay = 0.3f; // s the tuck must be unneeded before the collider returns
|
||
// 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 = 800f;
|
||
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.12f; // camera leading the body's eye by more than this = body held back: stick stops pushing that way
|
||
public const float ViewDeadzone = 0.02f; // m the head may lead the body before the view is eased back onto it
|
||
public const float ViewFollowTau = 0.04f; // s
|
||
public const float MaxViewLead = 0.04f; // hard: the head box reaches 2 cm in front of the eye
|
||
public const float FloorRecenterTau = 1f; // s
|
||
public const float HeadGuideFrequency = 5f; // Hz
|
||
public const float HeadGuideMass = 15f; // kg effective (head + part of the trunk)
|
||
public const float HeadGuideForce = 250f; // N
|
||
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.005f, 0.08f); // crown ~10 cm above the eyes, chin ~11 cm below (was 12.5 / 8.5)
|
||
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;
|
||
public Vector3 Ff;
|
||
}
|
||
|
||
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;
|
||
public Vector3 Ff;
|
||
}
|
||
|
||
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 FlexFf;
|
||
public float GuideCap = HandGuideForce;
|
||
public float ShoulderCap = ShoulderTorque, ElbowCap = ElbowTorque;
|
||
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
|
||
public bool LegsSolid => _legsSolid;
|
||
public bool Lifted => _lifted;
|
||
// Horizontal direction in which the body is held back from the camera (zero if not): the stick
|
||
// must not keep moving the play space that way.
|
||
public Vector3 HeldBack { get; private set; }
|
||
public bool Jumping => _jump != JumpPhase.None;
|
||
// 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 RigidBody3D _groundBody;
|
||
private Vector3 _groundPoint;
|
||
private float _tuckFree, _dt;
|
||
private PhysicsShapeQueryParameters3D _legsQuery;
|
||
|
||
private bool LegsSpaceFree(Vector3 center, Basis basis)
|
||
{
|
||
_legsQuery ??= new PhysicsShapeQueryParameters3D { Shape = Legs.GetChild<CollisionShape3D>(0).Shape, CollisionMask = WorldMask };
|
||
_legsQuery.Transform = new Transform3D(basis, center);
|
||
return GetWorld3D().DirectSpaceState.IntersectShape(_legsQuery, 1).Count == 0;
|
||
}
|
||
private Vector3 _goalVelocity;
|
||
private enum JumpPhase { None, Dip, Push, Flight }
|
||
private JumpPhase _jump;
|
||
private float _jumpT, _jumpRequest, _airT, _jumpFromY, _sinceLanding = float.MaxValue;
|
||
|
||
// Right A. Buffered, so a press just before touchdown still jumps.
|
||
public void Jump() => _jumpRequest = JumpBuffer;
|
||
|
||
private float _standingHip; // hips of the standing pose above the feet = the legs' full extension
|
||
|
||
private static float LowPass(float dt, float tau) => 1f - Mathf.Exp(-dt / tau);
|
||
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), friction: TorsoFriction);
|
||
float standingHip = pose.PelvisTarget.Y - (pose.Eye.Y - p.EyeHeight);
|
||
_standingHip = standingHip + _pelvisCenterLocal.Y; // of the pelvis BODY (what ground control holds)
|
||
_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);
|
||
// Explicit inertia: the collider is switched off when tucked, and a body without an
|
||
// enabled shape has no inertia — its 400 N·m drive then tumbled the whole pelvis
|
||
// (owner 2.1 "body spinning" when crouched).
|
||
Legs.Inertia = new Vector3(_legsHeight * _legsHeight + 0.04f, 0.28f * 0.28f + 0.04f, _legsHeight * _legsHeight + 0.08f) * (LegsMass / 12f);
|
||
_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), friction: TorsoFriction);
|
||
Head = MakeBody("Head", new BoxShape3D { Size = HeadSize * s }, 0.081f * m, BodyLayer, WorldMask, skin,
|
||
c * new Transform3D(Basis.Identity, neckPivot + headCenterFromPivot), friction: TorsoFriction);
|
||
// 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);
|
||
_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)), friction: LimbFriction);
|
||
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)), friction: LimbFriction);
|
||
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), friction: LimbFriction);
|
||
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;
|
||
_jump = JumpPhase.None;
|
||
_jumpRequest = 0f;
|
||
_sinceLanding = float.MaxValue;
|
||
}
|
||
|
||
// 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));
|
||
}
|
||
|
||
// NOTE (Box3D wrapper): friction is read only when the body's shapes are created — the material
|
||
// must be set before AddChild (setting it later silently does nothing).
|
||
private RigidBody3D MakeBody(string name, Shape3D shape, float mass, uint layer, uint mask, Color color, Transform3D at, bool visible = true, float friction = 1f)
|
||
{
|
||
RigidBody3D body = new() { Name = name, Mass = mass, CollisionLayer = layer, CollisionMask = mask, Transform = at, ContactMonitor = true, MaxContactsReported = 4,
|
||
PhysicsMaterialOverride = new PhysicsMaterial { Friction = friction } };
|
||
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).
|
||
// Only a horizontal TRANSLATION of the reach: solved in the chest TARGET frame (the
|
||
// muscles stay local). Aiming in the physical chest frame made the shoulder muscles
|
||
// feed chest rotation back into the chest — a slow twist that tracking jitter
|
||
// excited into "chest slipping out of control" (owner playtest 2.1).
|
||
Transform3D chestPhys = Chest.GlobalTransform.Orthonormalized();
|
||
Vector3 shPhys = chestPhys * (a.Shoulder.AnchorA + _girdle[side]);
|
||
Vector3 held = shPhys - sh;
|
||
held.Y = 0f;
|
||
Transform3D ctrl = _targets[a.Hand];
|
||
ArmSolver solver = _armSolvers[side];
|
||
solver.Solve(sh, ctrl * new Vector3(0f, 0f, WristOffset) - held, chest, chest.X * (side == 0 ? -1f : 1f),
|
||
ctrl.Basis, a.UpperLen, a.ForeLen, dt);
|
||
Vector3 d = solver.Wrist - sh;
|
||
Vector3 h = d.LengthSquared() > 1e-8f ? solver.Pole.Cross(d.Normalized()) : chest.X;
|
||
h = h.LengthSquared() > 1e-8f ? h.Normalized() : chest.X;
|
||
Vector3 u = (solver.Elbow - sh).Normalized(), f = (solver.Wrist - solver.Elbow).Normalized();
|
||
_targets[a.Upper] = new Transform3D(BoneBasis(u, h), sh + 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;
|
||
_dt = dt;
|
||
_floorYForHead = floorY;
|
||
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);
|
||
HeadGuide(s, 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 raw = b.HasPrev && dt > 0f ? (BodyMath.RotationError(target, b.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero;
|
||
b.Ff = b.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau));
|
||
b.PrevTarget = target;
|
||
b.HasPrev = true;
|
||
Vector3 w = (b.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 raw = a.PrevFlexTarget >= 0f && dt > 0f ? Mathf.Clamp((target - a.PrevFlexTarget) / dt, -MuscleMaxSpin, MuscleMaxSpin) : 0f;
|
||
a.FlexFf = Mathf.Lerp(a.FlexFf, raw, LowPass(dt, MuscleFeedForwardTau));
|
||
a.PrevFlexTarget = target;
|
||
float w = Mathf.Clamp(a.FlexFf + (target - ElbowFlex(side)) * ElbowKp, -MuscleMaxSpin, MuscleMaxSpin);
|
||
a.Elbow.SetParam(HingeJoint3D.Param.MotorTargetVelocity, HingeSign * w);
|
||
float cap = (w > 0f ? ElbowFlexTorque : ElbowTorque) * Mathf.Max(0.05f, Strength);
|
||
if (Mathf.Abs(cap - a.ElbowCap) > 0.5f)
|
||
{
|
||
a.ElbowCap = cap;
|
||
a.Elbow.SetParam(HingeJoint3D.Param.MotorMaxImpulse, cap);
|
||
}
|
||
}
|
||
|
||
// Head guide (Boneworks/BIMOS drive the head to the HMD): while standing on the ground, a force-
|
||
// capped PD pulls the character's eye onto the camera, so voluntary head motion (squats, leans,
|
||
// quick looks) doesn't leave the body lagging — a lag the riding view would otherwise turn into
|
||
// a permanent play-space offset. Off while airborne/climbing (no levitating by the head) or
|
||
// bracing on the hands (mantling, pushing off).
|
||
private void HeadGuide(TrueBodySolver s, float dt)
|
||
{
|
||
Vector3 goal = s.Eye + Vector3.Up * (_footY - _floorYForHead);
|
||
Vector3 raw = _haveHeadPrev && dt > 0f ? (goal - _headGoalPrev) / dt : Vector3.Zero;
|
||
_headGoalPrev = goal;
|
||
_haveHeadPrev = true;
|
||
if (raw.Length() > 10f)
|
||
raw = Vector3.Zero;
|
||
_headGoalVelocity = _headGoalVelocity.Lerp(raw, LowPass(dt, GroundFeedForwardTau));
|
||
// Only on your feet: hands bracing (mantling, pushing yourself) or crawling move the body by
|
||
// other means, and the view simply follows it.
|
||
if (!Grounded || Climbing || Braced || _jump != JumpPhase.None || _sinceLanding < AbsorbTime)
|
||
return;
|
||
float w = Mathf.Tau * HeadGuideFrequency;
|
||
Vector3 force = ((goal - Eye) * (w * w) + (_headGoalVelocity - Head.LinearVelocity) * (2f * w)) * HeadGuideMass;
|
||
Head.ApplyCentralForce(force.LimitLength(HeadGuideForce));
|
||
}
|
||
|
||
private Vector3 _headGoalPrev, _headGoalVelocity;
|
||
private bool _haveHeadPrev;
|
||
private float _floorYForHead;
|
||
|
||
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);
|
||
a.ShoulderCap = ShoulderTorque * k;
|
||
a.ElbowCap = ElbowTorque * 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) — not the floor:
|
||
// hands on the floor while crawling don't lift the body.
|
||
private static bool IsBraced(Arm a)
|
||
{
|
||
if (a.Held is StaticBody3D)
|
||
return true;
|
||
foreach (Node3D n in a.Hand.GetCollidingBodies())
|
||
if (n is StaticBody3D && !n.IsInGroup(PropGrabber.NoGrabGroup))
|
||
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);
|
||
// Tuck (collider off, folded into the pelvis): deep crouch/crawl, or the hands bracing to
|
||
// lift the body (mantle, climbing — the knees come up). It STAYS tucked until it has been
|
||
// unneeded for a while AND its untucked spot is free: re-enabling inside a ledge face is
|
||
// what shoved the body off again and again (owner 2.2 "legs trying to get off the
|
||
// ground and instantly coming back").
|
||
bool climbingHold = _arms[0].Held is StaticBody3D || _arms[1].Held is StaticBody3D;
|
||
_lifted = Braced && (Pelvis.GlobalPosition.Y > _supportGoalY + MantleLift || climbingHold);
|
||
bool deep = center + _legsHeight * 0.5f >= hip.Y + LegsOffAbove;
|
||
bool wantTuck = deep || _lifted || (!_legsSolid && Braced);
|
||
_tuckFree = wantTuck ? 0f : _tuckFree + _dt;
|
||
Vector3 untucked = new(hip.X, center, hip.Z);
|
||
bool solid = _legsSolid ? !wantTuck : _tuckFree > UntuckDelay && LegsSpaceFree(untucked, s.YawBasis);
|
||
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;
|
||
// Speed-limited: the legs dropping back out of the tuck at full drive speed kicked the pelvis
|
||
// up (the reaction) just as a landing dip was rising — a 5 cm bounce.
|
||
SetLinearVelocity(_legsDrive.Joint, frameA.Inverse() * ((desired - Legs.GlobalPosition) * LegsKp).LimitLength(LegsMaxSpeed));
|
||
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);
|
||
// Local muscle: relative to the chest's TARGET pose (never to the physical chest — that
|
||
// closes a rotation feedback loop through the arms).
|
||
Quaternion target = (_targets[Chest].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameA).Inverse()
|
||
* (_targets[a.Upper].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameB);
|
||
Vector3 raw = d.HasPrev && dt > 0f ? (BodyMath.RotationError(target, d.PrevTarget) / dt).LimitLength(MuscleMaxSpin) : Vector3.Zero;
|
||
d.Ff = d.Ff.Lerp(raw, LowPass(dt, MuscleFeedForwardTau));
|
||
d.PrevTarget = target;
|
||
d.HasPrev = true;
|
||
Vector3 spin = (d.Ff + BodyMath.RotationError(target, current) * ShoulderKp).LimitLength(MuscleMaxSpin);
|
||
SetAngularVelocity(d.Joint, spin);
|
||
// Which way is the muscle moving the arm? Rotating the bone direction downward = pulling.
|
||
Vector3 bone = (a.Upper.GlobalTransform.Basis.Y).Normalized();
|
||
Vector3 boneMotion = (frameA * spin).Cross(bone);
|
||
float cap = (boneMotion.Y < 0f ? ShoulderPullTorque : ShoulderTorque) * Mathf.Max(0.05f, Strength);
|
||
if (Mathf.Abs(cap - a.ShoulderCap) > 0.5f)
|
||
{
|
||
a.ShoulderCap = cap;
|
||
SetAngularCap(d.Joint, cap);
|
||
}
|
||
}
|
||
|
||
// 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".
|
||
// PUSH only (the hand wants to go further from the shoulder — straight arms pressing on a
|
||
// ledge): bone in compression. Pulling (shortening the arm: pull-ups, climbing) is muscle
|
||
// work — the asymmetric shoulder/elbow muscles do it, and one arm can't lift the body.
|
||
float align = axis.LengthSquared() > 1e-8f ? Mathf.Max(0f, err.Dot(axis.Normalized())) / (err.Length() + StructuralDeadband) : 0f;
|
||
// Structure only helps against something: a hand in free air reaching past its arm's
|
||
// length must not haul the whole body after it.
|
||
if (a.Held == null && a.Hand.GetContactCount() == 0)
|
||
align = 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;
|
||
_groundBody = null;
|
||
if (hit.Count > 0)
|
||
{
|
||
Vector3 point = (Vector3)hit["position"];
|
||
Vector3 normal = (Vector3)hit["normal"];
|
||
_groundBody = GodotObject.InstanceFromId((ulong)hit["collider_id"]) as RigidBody3D; // Box3D: "collider" is null
|
||
_groundPoint = point;
|
||
// Standing (or walking down a step) the legs keep contact over GroundProbeExtra; falling,
|
||
// they touch down only within their reach — the rest of the fall is stopped by the legs.
|
||
float reach = wasGrounded && _jump != JumpPhase.Flight ? GroundProbeExtra : LandingReach;
|
||
bool inReach = pos.Y - point.Y < hipAboveFloor + reach;
|
||
bool steep = normal.Y <= Mathf.Cos(Mathf.DegToRad(MaxSlopeDegrees));
|
||
if (_jump == JumpPhase.Flight && Pelvis.LinearVelocity.Y > 0f)
|
||
{
|
||
// Still rising off the ground: nothing to stand on yet.
|
||
}
|
||
else 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 || _lifted || 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 rawVelocity = _havePelvisPrev && dt > 0f ? (goalNow - _pelvisTargetPrev) / dt : Vector3.Zero;
|
||
_pelvisTargetPrev = goalNow;
|
||
_havePelvisPrev = true;
|
||
if (rawVelocity.Length() > 10f)
|
||
rawVelocity = Vector3.Zero;
|
||
_goalVelocity = _goalVelocity.Lerp(rawVelocity, LowPass(dt, GroundFeedForwardTau));
|
||
// Held back = the hips can't get to where the pose wants them (knee-high box, table edge,
|
||
// wall). Measured at the hips, not the head: the body tips over an obstacle, so the head
|
||
// may be right where the camera is while the hips are stuck.
|
||
Vector3 hipLag = new(target.Origin.X - pos.X, 0f, target.Origin.Z - pos.Z);
|
||
HeldBack = Grounded && hipLag.Length() > FreeSlack ? hipLag.Normalized() : Vector3.Zero;
|
||
Vector3 targetVelocity = _goalVelocity;
|
||
|
||
float weight = BodyMass * 9.8f;
|
||
if (Grounded && !wasGrounded && Pelvis.LinearVelocity.Y < -1.5f)
|
||
_sinceLanding = 0f;
|
||
_sinceLanding += dt;
|
||
UpdateJump(dt, wasGrounded);
|
||
if (_jump == JumpPhase.Push)
|
||
Grounded = true; // the feet push off the ground they were on (also over a just-left edge)
|
||
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;
|
||
if (_jump == JumpPhase.Dip)
|
||
goal.Y = Mathf.Min(goal.Y, _jumpFromY);
|
||
float w = Mathf.Tau * GroundFrequency;
|
||
Vector3 accel = (goal - pos) * (w * w) + (targetVelocity - Pelvis.LinearVelocity) * (2f * GroundDamping * w);
|
||
Vector3 force = accel * BodyMass;
|
||
// Vertical as a rate-limited velocity servo: full leg strength brakes a landing, but
|
||
// the legs raise the body at most MaxRiseSpeed — they can push, not pull, so a hard push
|
||
// toward a higher stance (onto a ledge) would otherwise overshoot into a jump.
|
||
// Rising, the legs can only stop the body by letting gravity do it (support ≥ 0): ease the
|
||
// rise so it can still be stopped at the stance with ~0.7 g, or it overshoots (a bounce).
|
||
float below = goal.Y - pos.Y;
|
||
float stoppable = below > 0f ? Mathf.Sqrt(2f * 0.7f * 9.8f * below) : 0f;
|
||
float riseVelocity = Mathf.Clamp(below * w, -3f, Mathf.Min(MaxRiseSpeed, stoppable)) + targetVelocity.Y;
|
||
float verticalAccel = (riseVelocity - Pelvis.LinearVelocity.Y) * (2f * GroundDamping * w);
|
||
float support = Mathf.Clamp(verticalAccel * BodyMass + weight, 0f, LegStrength * weight);
|
||
if (_jump == JumpPhase.Push)
|
||
support = JumpForce * weight;
|
||
float carry = Mathf.Max(GroundFriction * support, _legsSolid ? 0f : CrawlCarry);
|
||
// Hips held back by an obstacle: lean on it, don't bulldoze — full leg force tipped the
|
||
// body over knee-high boxes and steep slopes (and the riding view tipped with it).
|
||
if (HeldBack != Vector3.Zero)
|
||
carry = Mathf.Min(carry, BlockedPush);
|
||
Vector3 horizontal = new Vector3(force.X, 0f, force.Z).LimitLength(carry);
|
||
Pelvis.ApplyCentralForce(horizontal + Vector3.Up * support);
|
||
// Newton's third law: standing on something movable (seesaw, plank, crate) pushes it
|
||
// down — the feet aren't physical, so the reaction is applied where the ground ray hit.
|
||
if (_groundBody != null && IsInstanceValid(_groundBody))
|
||
_groundBody.ApplyForce(-(horizontal + Vector3.Up * support), _groundPoint - _groundBody.GlobalPosition);
|
||
}
|
||
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);
|
||
}
|
||
}
|
||
|
||
// Jump state: request → Dip (countermovement, the support goal drops) → Push (full leg force up to
|
||
// extension) → Flight (no ground until falling again) → None on touchdown.
|
||
private void UpdateJump(float dt, bool wasGrounded)
|
||
{
|
||
_airT = Grounded ? 0f : _airT + dt;
|
||
_jumpRequest -= dt;
|
||
_jumpT += dt;
|
||
float pos = Pelvis.GlobalPosition.Y;
|
||
float top = _footY + _standingHip + JumpExtension;
|
||
switch (_jump)
|
||
{
|
||
case JumpPhase.None:
|
||
if (_jumpRequest > 0f && (Grounded || _airT < CoyoteTime) && !Climbing)
|
||
{
|
||
_jumpRequest = 0f;
|
||
_jumpT = 0f;
|
||
// Stroke at least JumpDip + JumpExtension: from standing, dip first.
|
||
_jumpFromY = Mathf.Max(top - JumpMaxStroke, Mathf.Min(pos, top - JumpDip - JumpExtension));
|
||
_jump = pos > _jumpFromY + 0.02f && Grounded ? JumpPhase.Dip : JumpPhase.Push;
|
||
_jumpFromY = Mathf.Min(_jumpFromY, pos);
|
||
}
|
||
break;
|
||
case JumpPhase.Dip:
|
||
if (pos <= _jumpFromY + 0.02f || _jumpT > 0.2f || !Grounded)
|
||
{
|
||
_jump = JumpPhase.Push;
|
||
_jumpFromY = pos;
|
||
_jumpT = 0f;
|
||
}
|
||
break;
|
||
case JumpPhase.Push:
|
||
if (pos >= top || pos - _jumpFromY >= JumpMaxStroke || _jumpT > 0.35f)
|
||
{
|
||
_jump = JumpPhase.Flight;
|
||
_jumpT = 0f;
|
||
}
|
||
break;
|
||
case JumpPhase.Flight:
|
||
if (Grounded && _jumpT > 0.05f)
|
||
_jump = JumpPhase.None;
|
||
break;
|
||
}
|
||
}
|
||
|
||
private Drive _balance;
|
||
private float _balanceCap;
|
||
|
||
// ---------------------------------------------------------------- play space
|
||
|
||
// How the driver should move its play-space origin after the last physics step. ONE rule, always,
|
||
// in all directions (Boneworks/BIMOS: the rig rides the body): the camera rides the character's
|
||
// eye. Head motion leads the body by at most ViewDeadzone (eased) / MaxViewLead (hard) — less
|
||
// than the head box reaches in front of the eye, so the view stays inside the head. A sphere
|
||
// swept eye→camera snaps the view back at once when geometry is in the way.
|
||
// This does NOT feed back into the pose: the True Body reads the head relative to the play-space
|
||
// floor (crouch = local head height) and ground control re-bases its goal onto the real ground
|
||
// (footY − floorY), so moving the play space vertically changes neither. (2.3 split height off
|
||
// to a separate floor-follow out of fear of that loop; the two then fought over the same height,
|
||
// and landings left the view hanging above a sinking body — owner 2.4.)
|
||
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);
|
||
Vector3 rest = lead + correction;
|
||
if (rest.Length() > ViewDeadzone)
|
||
correction -= rest * (1f - ViewDeadzone / rest.Length()) * (1f - Mathf.Exp(-dt / ViewFollowTau));
|
||
rest = lead + correction;
|
||
if (rest.Length() > MaxViewLead)
|
||
correction -= rest * (1f - MaxViewLead / rest.Length());
|
||
}
|
||
// Slow re-centering of the play-space floor onto the real ground while standing: removes the
|
||
// small height offsets the riding view picks up (a transient lag, a landing). The pose and the
|
||
// head guide are re-based on the real ground, so this only brings the camera onto the eye —
|
||
// the same direction as the follow above, never against it.
|
||
if (Grounded)
|
||
correction.Y += (_footY - floorY) * (1f - Mathf.Exp(-dt / FloorRecenterTau));
|
||
// 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).
|
||
// Horizontally only — vertically the goals are re-based on the real ground (− floorY), so
|
||
// they don't move with the play space; shifting them too turned the view riding a landing
|
||
// dip into a ~1 m/s upward feed-forward (a 15 cm bounce).
|
||
Vector3 flat = new(correction.X, 0f, correction.Z);
|
||
_pelvisTargetPrev += flat;
|
||
_headGoalPrev += flat;
|
||
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);
|
||
}
|
||
}
|
||
}
|