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>
59 lines
2.6 KiB
C#
59 lines
2.6 KiB
C#
using Godot;
|
|
|
|
// Shared math for the chase-driven player bodies (hands, torso, arm segments).
|
|
public static class BodyMath
|
|
{
|
|
// Shortest-arc rotation taking `current` to `target`, as axis * angle (radians), world space.
|
|
// Never use Quaternion.GetAngle()/GetAxis() for servo errors: GetAngle() is 2*acos(W) and
|
|
// returns NaN when float rounding puts W at 1.0000001 (the Stage-1 chest collapse), and
|
|
// GetAxis() returns the raw un-normalized xyz once W > 1 - 1e-6 (servo gain collapses
|
|
// ~1000x inside 0.16 deg). atan2 is exact near zero and can't leave its domain.
|
|
public static Vector3 RotationError(Quaternion target, Quaternion current)
|
|
{
|
|
Quaternion q = target * current.Inverse();
|
|
if (q.W < 0f)
|
|
q = new Quaternion(-q.X, -q.Y, -q.Z, -q.W);
|
|
Vector3 v = new(q.X, q.Y, q.Z);
|
|
float s = v.Length();
|
|
if (s < 1e-9f)
|
|
return Vector3.Zero;
|
|
return v * (2f * Mathf.Atan2(s, q.W) / s);
|
|
}
|
|
|
|
// The horizontal direction a head faces, robust past vertical. The visor (-Z) projected flat
|
|
// flips 180° once you look down (or up) beyond vertical — chin to chest while crouching, or
|
|
// looking at your own feet — and everything built on it (torso yaw, stick direction) turned
|
|
// around. Looking down, the top of the head (+Y) points the way you face; looking up it points
|
|
// back. So: when the flat visor disagrees with the flat crown, the head is past vertical and the
|
|
// visor is reversed; near vertical the crown alone carries the direction. Zero if undefined.
|
|
public static Vector3 HeadFacing(Basis head)
|
|
{
|
|
Vector3 forward = -head.Z, up = head.Y;
|
|
Vector3 visor = new(forward.X, 0f, forward.Z);
|
|
Vector3 crown = new Vector3(up.X, 0f, up.Z) * -forward.Y;
|
|
if (visor.Dot(crown) < 0f)
|
|
visor = -visor;
|
|
Vector3 f = visor + crown * 0.5f;
|
|
return f.LengthSquared() > 1e-6f ? f.Normalized() : Vector3.Zero;
|
|
}
|
|
|
|
// Yaw (about +Y, Godot convention: 0 = facing -Z) of a horizontal direction.
|
|
public static float YawOf(Vector3 flat) => Mathf.Atan2(-flat.X, -flat.Z);
|
|
|
|
public static bool IsFinite(RigidBody3D body) =>
|
|
body.GlobalTransform.IsFinite() && body.LinearVelocity.IsFinite() && body.AngularVelocity.IsFinite();
|
|
|
|
// Escape hatch: a non-finite body is unrecoverable for the solver and floods the renderer
|
|
// with !v.is_finite() errors — put it back on its target, motionless. Returns true if reset.
|
|
public static bool ResetIfNonFinite(RigidBody3D body, Transform3D target, string who)
|
|
{
|
|
if (IsFinite(body))
|
|
return false;
|
|
body.GlobalTransform = target;
|
|
body.LinearVelocity = Vector3.Zero;
|
|
body.AngularVelocity = Vector3.Zero;
|
|
GD.PushWarning($"{who}: non-finite body state, reset to target");
|
|
return true;
|
|
}
|
|
}
|