Body step 2.1: climbing, mantling, crawling, legs, playground, 144 Hz
- 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>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed
1 parent
d83273dd2f
commit
2aff2b61da
26 files changed
+1243
-70
No files matched your search
+245
-21
@@ -33,6 +33,14 @@ public partial class PlayerRagdoll : Node3D
|
||||
// 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
|
||||
@@ -40,6 +48,18 @@ public partial class PlayerRagdoll : Node3D
|
||||
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;
|
||||
@@ -48,6 +68,8 @@ public partial class PlayerRagdoll : Node3D
|
||||
// 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.
|
||||
@@ -92,8 +114,9 @@ public partial class PlayerRagdoll : Node3D
|
||||
public HingeJoint3D Elbow;
|
||||
public Drive Shoulder, Guide;
|
||||
public float UpperLen, ForeLen;
|
||||
public RigidBody3D Held;
|
||||
public float PrevFlexTarget = -1f;
|
||||
public float GuideCap = HandGuideForce;
|
||||
public PhysicsBody3D Held;
|
||||
}
|
||||
|
||||
public RigidBody3D Pelvis { get; private set; }
|
||||
@@ -101,7 +124,11 @@ public partial class PlayerRagdoll : Node3D
|
||||
public RigidBody3D Head { get; private set; }
|
||||
public bool Built { get; private set; }
|
||||
public bool Grounded { get; private set; }
|
||||
public float GroundY { get; private set; }
|
||||
public 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
|
||||
@@ -115,6 +142,14 @@ public partial class PlayerRagdoll : Node3D
|
||||
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;
|
||||
|
||||
@@ -157,14 +192,21 @@ public partial class PlayerRagdoll : Node3D
|
||||
_eyeLocal -= headCenterFromPivot; // eye relative to the head body's centre
|
||||
|
||||
Color skin = new(0.85f, 0.72f, 0.6f), shirt = new(0.3f, 0.45f, 0.7f), pants = new(0.25f, 0.27f, 0.32f);
|
||||
Pelvis = MakeBody("Pelvis", new BoxShape3D { Size = pelvisSize }, (0.142f + 0.139f + 0.322f) * m, BodyLayer, WorldMask, pants,
|
||||
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 });
|
||||
@@ -175,6 +217,10 @@ public partial class PlayerRagdoll : Node3D
|
||||
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);
|
||||
@@ -217,6 +263,9 @@ public partial class PlayerRagdoll : Node3D
|
||||
|
||||
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)");
|
||||
}
|
||||
@@ -249,11 +298,11 @@ public partial class PlayerRagdoll : Node3D
|
||||
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)
|
||||
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 };
|
||||
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)
|
||||
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 });
|
||||
@@ -337,14 +386,17 @@ public partial class PlayerRagdoll : Node3D
|
||||
// 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)
|
||||
private void ComputeTargets(TrueBodySolver s, Transform3D left, Transform3D right, bool physicalArms = false, float dt = 0f)
|
||||
{
|
||||
Basis chest = s.ChestBasis;
|
||||
Basis pelvis = s.YawBasis * new Basis(Vector3.Right, -s.Lean * PelvisLeanShare);
|
||||
// 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;
|
||||
@@ -368,9 +420,33 @@ public partial class PlayerRagdoll : Node3D
|
||||
_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
|
||||
@@ -379,7 +455,7 @@ public partial class PlayerRagdoll : Node3D
|
||||
{
|
||||
if (!Built)
|
||||
return;
|
||||
ComputeTargets(s, left, right);
|
||||
ComputeTargets(s, left, right, physicalArms: true, dt);
|
||||
foreach (RigidBody3D b in _bodies)
|
||||
{
|
||||
if (BodyMath.ResetIfNonFinite(b, Target(b), "PlayerRagdoll"))
|
||||
@@ -401,14 +477,20 @@ public partial class PlayerRagdoll : Node3D
|
||||
|
||||
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
|
||||
@@ -461,6 +543,72 @@ public partial class PlayerRagdoll : Node3D
|
||||
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).
|
||||
@@ -474,7 +622,8 @@ public partial class PlayerRagdoll : Node3D
|
||||
SetLinearVelocity(d.Joint, frameA.Inverse() * (err * GirdleKp));
|
||||
|
||||
Quaternion current = frameA.Inverse() * (Rot(a.Upper) * d.FrameB);
|
||||
Quaternion target = (_targets[Chest].Basis.Orthonormalized().GetRotationQuaternion() * d.FrameA).Inverse()
|
||||
// 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;
|
||||
@@ -488,12 +637,24 @@ public partial class PlayerRagdoll : Node3D
|
||||
Transform3D chest = Chest.GlobalTransform;
|
||||
Vector3 anchor = chest * a.Guide.AnchorA;
|
||||
Vector3 anchorVelocity = Chest.LinearVelocity + Chest.AngularVelocity.Cross(anchor - Chest.GlobalPosition);
|
||||
Vector3 desired = (targetVelocity + (target.Origin - a.Hand.GlobalPosition) * HandGuideKp).LimitLength(HandGuideMaxSpeed);
|
||||
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 err = BodyMath.RotationError(target.Basis.GetRotationQuaternion(), Rot(a.Hand));
|
||||
Vector3 wWorld = (err * HandGuideAngularKp).LimitLength(MuscleMaxSpin * 2f) - Chest.AngularVelocity;
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -506,20 +667,42 @@ public partial class PlayerRagdoll : Node3D
|
||||
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"];
|
||||
if (pos.Y - point.Y < hipAboveFloor + GroundProbeExtra && normal.Y > 0.6f)
|
||||
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;
|
||||
GroundY = point.Y;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 targetVelocity = _havePelvisPrev && dt > 0f ? (target.Origin - _pelvisTargetPrev) / dt : Vector3.Zero;
|
||||
_pelvisTargetPrev = target.Origin;
|
||||
// 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;
|
||||
@@ -529,13 +712,25 @@ public partial class PlayerRagdoll : Node3D
|
||||
{
|
||||
// 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);
|
||||
Vector3 horizontal = new Vector3(force.X, 0f, force.Z).LimitLength(GroundFriction * support);
|
||||
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);
|
||||
@@ -576,20 +771,44 @@ public partial class PlayerRagdoll : Node3D
|
||||
float[] fractions = GetWorld3D().DirectSpaceState.CastMotion(_cameraQuery);
|
||||
float safe = fractions.Length > 0 ? fractions[0] : 1f;
|
||||
if (safe < 1f)
|
||||
{
|
||||
correction = -lead * (1f - safe);
|
||||
else if (lead.Length() > FreeSlack)
|
||||
correction = -lead * (1f - FreeSlack / lead.Length());
|
||||
// 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)
|
||||
@@ -601,6 +820,9 @@ public partial class PlayerRagdoll : Node3D
|
||||
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)
|
||||
@@ -620,11 +842,13 @@ public partial class PlayerRagdoll : Node3D
|
||||
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, RigidBody3D prop)
|
||||
public void SetHeld(int side, PhysicsBody3D prop)
|
||||
{
|
||||
if (!Built)
|
||||
return;
|
||||
|
||||
Reference in new issue
Block a user