Body physics Stage 1: PhysicsTorso chase chest (30 kg PD + yaw servo), collision layers reworked (3=body, 4=hands, no self-collision), BodyRig visual-only, BodySpike regression (PASS 3x, all spikes green)

scripts/BodySpike.cs

@@ -0,0 +1,328 @@
+using System.Collections.Generic;
+using Godot;
+
+public partial class BodySpike : Node3D
+{
+    private enum Phase { Settle, Pairs, Track, Yaw, Impact, Snap, Upright }
+
+    private const float PhaseSettle = 0.8f;
+    private const float PhasePairs1 = 1.3f;
+    private const float PhasePairs2 = 1.7f;
+    private const float PhaseTrack = 3.5f;
+    private const float PhaseYaw = 2f;
+    private const float PhaseImpact = 2f;
+    private const float PhaseSnap = 0.3f;
+    private const float PhaseUpright = 2f;
+
+    private const float TrackPosLimit = 0.06f;
+    private const float TrackSpeedLimit = 8f;
+    private const float YawPeakLimitDeg = 15f;
+    private const float YawSettleLimitDeg = 4f;
+    private const float ImpactDipLimit = 0.2f;
+    private const float RecoverLimit = 0.02f;
+    private const float ImpactorMinDistLimit = 0.1f;
+    private const float ImpactorSpeedAfterHitLimit = 1.5f;
+    private const float SnapLimit = 0.05f;
+    private const float UprightLimitDeg = 3f;
+    private const float YawHoldLimitDeg = 5f;
+
+    private PhysicsTorso _torso;
+    private Node3D _head;
+    private RigidBody3D _ball;
+    private RigidBody3D _handProbe;
+    private RigidBody3D _prop;
+    private RigidBody3D _impactor;
+
+    private Phase _phase = Phase.Settle;
+    private float _phaseStart;
+    private float _elapsed;
+    private readonly List<string> _resultNames = new();
+    private readonly List<bool> _resultOk = new();
+
+    private Vector3 _chestRef;
+    private bool _chestStillDuringHandDrop;
+    private float _chestDropDip;
+    private float _maxTrackErr;
+    private float _maxTrackSpeed;
+    private float _maxYawErrAfterRamp;
+    private float _yawErrEnd;
+    private float _maxImpactDip;
+    private float _minImpactorDist = float.MaxValue;
+    private float _impactorTouchTime = -1f;
+    private float _impactorSpeedAfterHit = -1f;
+
+    public override void _Ready()
+    {
+        BuildWorld();
+        _head = new Node3D { Position = new Vector3(0f, 1.6f, 0f) };
+        AddChild(_head);
+
+        _torso = new PhysicsTorso { HeadSource = _head };
+        AddChild(_torso);
+        GD.Print("BODY: body spike scene ready");
+    }
+
+    private void BuildWorld()
+    {
+        StaticBody3D ground = new() { CollisionLayer = 1, CollisionMask = 0 };
+        ground.AddChild(new CollisionShape3D
+        {
+            Shape = new BoxShape3D { Size = new Vector3(20f, 0.1f, 20f) },
+            Position = new Vector3(0f, -0.05f, 0f),
+        });
+        AddChild(ground);
+
+        _ball = new RigidBody3D
+        {
+            Mass = 1f,
+            Position = new Vector3(-2f, 0.4f, 0f),
+            CollisionLayer = 1,
+            CollisionMask = 1,
+            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.8f },
+        };
+        _ball.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.05f } });
+        AddChild(_ball);
+
+        _handProbe = new RigidBody3D
+        {
+            Mass = 0.4f,
+            Position = new Vector3(0f, 1.7f, 0.12f),
+            CollisionLayer = 0b1000,
+            CollisionMask = 0b1001,
+            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0f, Friction = 0.8f },
+        };
+        _handProbe.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.035f, 0.09f, 0.12f) } });
+        AddChild(_handProbe);
+
+        _prop = new RigidBody3D
+        {
+            Mass = 2f,
+            Position = new Vector3(0f, 1.9f, 0.12f),
+            CollisionLayer = 1,
+            CollisionMask = 0b1101,
+            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.3f, Friction = 0.8f },
+        };
+        _prop.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } });
+        AddChild(_prop);
+        _prop.Freeze = true;
+        _prop.Visible = false;
+
+        _impactor = new RigidBody3D
+        {
+            Mass = 5f,
+            GravityScale = 0f,
+            CollisionLayer = 1,
+            CollisionMask = 0b1101,
+            PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.6f },
+        };
+        _impactor.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } });
+        AddChild(_impactor);
+        _impactor.Freeze = true;
+    }
+
+    public override void _PhysicsProcess(double delta)
+    {
+        float dt = (float)delta;
+        _elapsed += dt;
+        float phaseT = _elapsed - _phaseStart;
+
+        Vector3 p = _torso.Body.GlobalPosition;
+        if (float.IsNaN(p.X) || float.IsNaN(p.Y) || float.IsNaN(p.Z))
+        {
+            Record("no NaN in body state", false);
+            Finish();
+            return;
+        }
+
+        switch (_phase)
+        {
+            case Phase.Settle:
+                if (phaseT >= PhaseSettle)
+                {
+                    _chestRef = _torso.Body.GlobalPosition;
+                    StartPhase(Phase.Pairs);
+                    GD.Print("BODY: pairs phase — one-way floor, hand-through-chest exclusion, prop contact");
+                }
+                break;
+
+            case Phase.Pairs:
+            {
+                if (phaseT >= PhasePairs1 && _prop.Freeze)
+                {
+                    _chestStillDuringHandDrop = _torso.Body.GlobalPosition.DistanceTo(_chestRef) < 0.02f;
+                    _chestRef = _torso.Body.GlobalPosition;
+                    _chestDropDip = 0f;
+                    _prop.Freeze = false;
+                    _prop.Visible = true;
+                    GD.Print("BODY: dropping 2 kg ball onto chest top");
+                }
+                _chestDropDip = Mathf.Max(_chestDropDip, _torso.Body.GlobalPosition.DistanceTo(_chestRef));
+                if (phaseT >= PhasePairs1 + PhasePairs2)
+                    FinishPairs();
+                break;
+            }
+
+            case Phase.Track:
+            {
+                float t = phaseT;
+                Vector3 headPos = new(0.6f * Mathf.Sin(1.1f * t), 1.6f, 0.8f * Mathf.Sin(0.7f * t));
+                Basis headBasis = new(new Quaternion(Vector3.Up, 0.6f * Mathf.Sin(0.5f * t)));
+                _head.GlobalTransform = new Transform3D(headBasis, headPos);
+                if (phaseT > 0.5f)
+                {
+                    _maxTrackErr = Mathf.Max(_maxTrackErr, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
+                    _maxTrackSpeed = Mathf.Max(_maxTrackSpeed, _torso.Body.LinearVelocity.Length());
+                }
+                if (phaseT >= PhaseTrack)
+                {
+                    Record($"track error max {_maxTrackErr:F3} m (limit {TrackPosLimit})", _maxTrackErr < TrackPosLimit);
+                    Record($"track speed max {_maxTrackSpeed:F2} m/s (limit {TrackSpeedLimit})", _maxTrackSpeed < TrackSpeedLimit);
+                    StartPhase(Phase.Yaw);
+                    GD.Print("BODY: yaw phase — head turns 90 deg over 1 s");
+                }
+                break;
+            }
+
+            case Phase.Yaw:
+            {
+                float yaw = Mathf.Pi / 2f * Mathf.Min(phaseT / 1f, 1f);
+                _head.GlobalTransform = new Transform3D(new Basis(new Quaternion(Vector3.Up, yaw)), _head.GlobalPosition);
+                float err = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis));
+                if (phaseT > 0.4f)
+                    _maxYawErrAfterRamp = Mathf.Max(_maxYawErrAfterRamp, err);
+                if (phaseT > PhaseYaw - 0.5f)
+                    _yawErrEnd = Mathf.Max(_yawErrEnd, err);
+                if (phaseT >= PhaseYaw)
+                {
+                    Record($"yaw error peak {_maxYawErrAfterRamp:F1} deg (limit {YawPeakLimitDeg})", _maxYawErrAfterRamp < YawPeakLimitDeg);
+                    Record($"yaw settled {_yawErrEnd:F1} deg (limit {YawSettleLimitDeg})", _yawErrEnd < YawSettleLimitDeg);
+                    StartPhase(Phase.Impact);
+                    GD.Print("BODY: impact phase — 5 kg ball at 3 m/s into the chest");
+                }
+                break;
+            }
+
+            case Phase.Impact:
+            {
+                _maxImpactDip = Mathf.Max(_maxImpactDip, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
+                _minImpactorDist = Mathf.Min(_minImpactorDist, _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition));
+                if (_impactorTouchTime >= 0f && _impactorSpeedAfterHit < 0f && phaseT > _impactorTouchTime + 0.2f)
+                    _impactorSpeedAfterHit = _impactor.LinearVelocity.Length();
+                if (phaseT >= PhaseImpact)
+                    FinishImpact();
+                break;
+            }
+
+            case Phase.Snap:
+            {
+                if (phaseT >= PhaseSnap)
+                {
+                    float err = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition);
+                    Record($"snap re-locate (err {err:F3} m, limit {SnapLimit})", err < SnapLimit);
+                    StartPhase(Phase.Upright);
+                    _torso.Body.AngularVelocity += _torso.Body.GlobalTransform.Basis.X * 6f;
+                    GD.Print("BODY: upright phase — pitch impulse, body must self-right");
+                }
+                break;
+            }
+
+            case Phase.Upright:
+            {
+                if (phaseT >= PhaseUpright)
+                    FinishUpright();
+                break;
+            }
+        }
+
+        if (_phase == Phase.Impact && _impactorTouchTime < 0f
+            && _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition) < 0.25f)
+            _impactorTouchTime = _elapsed - _phaseStart;
+    }
+
+    private void StartPhase(Phase phase)
+    {
+        _phase = phase;
+        _phaseStart = _elapsed;
+        if (phase == Phase.Impact)
+        {
+            _maxImpactDip = 0f;
+            _minImpactorDist = float.MaxValue;
+            _impactorTouchTime = -1f;
+            _impactorSpeedAfterHit = -1f;
+            _impactor.Freeze = false;
+            Vector3 chest = _torso.Body.GlobalPosition;
+            Vector3 fwd = -_torso.Body.GlobalTransform.Basis.Z;
+            _impactor.GlobalPosition = chest + fwd * 0.5f + Vector3.Up * 0.05f;
+            _impactor.LinearVelocity = -fwd * 3f;
+            _impactor.Visible = true;
+            GD.Print($"BODY: impactor armed freeze={_impactor.Freeze} pos={_impactor.GlobalPosition} v={_impactor.LinearVelocity}");
+        }
+    }
+
+    private void FinishPairs()
+    {
+        float chestY = _torso.Body.GlobalPosition.Y;
+        bool handBelow = _handProbe.GlobalPosition.Y < chestY - 0.4f;
+        Record($"hand-config body falls through own chest (hand y {_handProbe.GlobalPosition.Y:F2}, chest y {chestY:F2})", handBelow);
+        Record("chest undisturbed while hand-config body falls through", _chestStillDuringHandDrop);
+        bool propContact = _chestDropDip > 0.0001f;
+        bool propBounded = _chestDropDip < 0.1f;
+        Record($"dropped ball transfers contact into chest (dip {_chestDropDip:F4} m)", propContact && propBounded);
+        _prop.LinearVelocity = Vector3.Zero;
+        _prop.GlobalPosition = new Vector3(-2f, 0.3f, 1f);
+        StartPhase(Phase.Track);
+        GD.Print("BODY: track phase — head walks a smooth path");
+    }
+
+    private void FinishImpact()
+    {
+        Record($"impact chest dip max {_maxImpactDip:F3} m (limit {ImpactDipLimit})", _maxImpactDip < ImpactDipLimit);
+        float recovered = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition);
+        Record($"impact recovery {recovered:F3} m (limit {RecoverLimit})", recovered < RecoverLimit);
+        Record($"impactor never passed through (min dist {_minImpactorDist:F3} m, limit {ImpactorMinDistLimit})", _minImpactorDist > ImpactorMinDistLimit);
+        Record($"impactor slowed after hit (|v| {_impactorSpeedAfterHit:F2} m/s, limit {ImpactorSpeedAfterHitLimit})",
+            _impactorSpeedAfterHit >= 0f && _impactorSpeedAfterHit < ImpactorSpeedAfterHitLimit);
+        StartPhase(Phase.Snap);
+        _head.GlobalPosition += new Vector3(2.5f, 0f, 0f);
+        GD.Print("BODY: snap phase — head teleported 2.5 m");
+    }
+
+    private void FinishUpright()
+    {
+        float up = _torso.Body.GlobalTransform.Basis.Y.Dot(Vector3.Up);
+        float uprightDeg = Mathf.RadToDeg(Mathf.Acos(Mathf.Clamp(up, -1f, 1f)));
+        float yawErr = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis));
+        Record($"self-right to upright {uprightDeg:F1} deg (limit {UprightLimitDeg})", uprightDeg < UprightLimitDeg);
+        Record($"yaw held through tumble {yawErr:F1} deg (limit {YawHoldLimitDeg})", yawErr < YawHoldLimitDeg);
+        Finish();
+    }
+
+    private static float YawErrorDeg(Basis bodyBasis, Basis headBasis)
+    {
+        Vector3 bodyFwd = -bodyBasis.Z;
+        Vector3 headFwd = -headBasis.Z;
+        bodyFwd.Y = 0f;
+        headFwd.Y = 0f;
+        if (bodyFwd.LengthSquared() < 0.0001f || headFwd.LengthSquared() < 0.0001f)
+            return 0f;
+        bodyFwd = bodyFwd.Normalized();
+        headFwd = headFwd.Normalized();
+        return Mathf.RadToDeg(Mathf.Atan2(bodyFwd.Cross(headFwd).Dot(Vector3.Up), bodyFwd.Dot(headFwd)));
+    }
+
+    private void Record(string name, bool ok)
+    {
+        _resultNames.Add(name);
+        _resultOk.Add(ok);
+        GD.Print($"BODY: {name} → {(ok ? "PASS" : "FAIL")}");
+    }
+
+    private void Finish()
+    {
+        bool overall = true;
+        foreach (bool ok in _resultOk)
+            overall &= ok;
+        GD.Print($"BODY: overall {(overall ? "PASS — physical torso functional on Box3D" : "CHECK — inspect failures above")}");
+        GetTree().Quit();
+    }
+}