FoxyzandClaude Opus 5.5
cb518999e5
- BodyMath.RotationError: atan2-based shortest-arc error (no NaN, no GetAxis gain collapse inside 0.16 deg); used by PhysicsTorso orientation servo and PhysicsHands rotation lock - BodyMath.ResetIfNonFinite escape hatch on torso + hands - PhysicsHands.Drive extracted (static) so spikes run the real hand chase - CollapseSpike: deterministic repro (slow yaw drift across a parked chest, 7/40 NaN on old code) + solver-stacking scenarios (seated chest in table, two-hand-welded 50 kg anvil, real PropShop) — 49/49 clean after fix, x3; all other spikes PASS x3 - Failure brief RESOLUTION section; HANDOFF/AGENTS updated Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
39 lines
1.7 KiB
C#
39 lines
1.7 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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|