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/BodyRig.cs

@@ -4,8 +4,6 @@ public partial class BodyRig : Node3D
 {
     private const float HeadBackOffset = 0.06f;
     private const float HeadSize = 0.15f;
-    private const float TorsoDrop = 0.42f;
-    private const float TorsoBackOffset = 0.06f;
     private const float ShoulderRise = 0.25f;
     private const float ReachBudgetMax = 0.25f;
     private const float ReachVerticalMin = -0.05f;
@@ -17,16 +15,13 @@ public partial class BodyRig : Node3D
     private const float SoftIKFactor = 0.98f;
     private const float PoleHandWeight = 0.25f;
     private const float PoleSmoothRate = 10f;
-    private const float TorsoYawHandsWeight = 0.6f;
-    private const float TorsoYawForwardness = 0.3f;
-    private const float TorsoYawSmoothRate = 12f;
 
     private XRCamera3D _camera;
     private XRController3D _left;
     private XRController3D _right;
+    private PhysicsTorso _torso;
 
     private MeshInstance3D _headMesh;
-    private MeshInstance3D _torsoMesh;
     private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2];
     private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2];
 
@@ -37,7 +32,6 @@ public partial class BodyRig : Node3D
     private Vector3 _torsoOffset = Vector3.Zero;
     private Basis _yawBasis = Basis.Identity;
     private Basis _torsoBasis = Basis.Identity;
-    private float _torsoYaw;
     private readonly Vector3[] _lastPole = new Vector3[2];
 
     public override void _Ready()
@@ -45,9 +39,9 @@ public partial class BodyRig : Node3D
         _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
         _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
         _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
+        _torso = GetNode<PhysicsTorso>("../PhysicsTorso");
 
         _headMesh = MakeMesh(new BoxMesh { Size = new Vector3(HeadSize, HeadSize, HeadSize) }, new Color(0.8f, 0.8f, 0.8f));
-        _torsoMesh = MakeMesh(new BoxMesh { Size = new Vector3(0.22f, 0.5f, 0.13f) }, new Color(0.4f, 0.45f, 0.55f));
         _upperArmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
         _upperArmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
         _forearmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));
@@ -67,7 +61,6 @@ public partial class BodyRig : Node3D
     private void SetRigVisible(bool visible)
     {
         _headMesh.Visible = visible;
-        _torsoMesh.Visible = visible;
         foreach (MeshInstance3D m in _upperArmMeshes)
             m.Visible = visible;
         foreach (MeshInstance3D m in _forearmMeshes)
@@ -86,15 +79,14 @@ public partial class BodyRig : Node3D
                 return;
             Calibrate(headHeight);
         }
+        if (_torso.Body == null)
+            return;
 
         Vector3 headPos = _camera.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
         _headMesh.GlobalTransform = new Transform3D(_yawBasis, headPos);
 
-        UpdateTorsoYaw(headPos, dt);
-
-        Vector3 torsoBack = _torsoBasis * Vector3.Back;
-        Vector3 torsoCenter = headPos + Vector3.Down * TorsoDrop + torsoBack * TorsoBackOffset + _torsoOffset;
-        _torsoMesh.GlobalTransform = new Transform3D(_torsoBasis, torsoCenter);
+        _torsoBasis = _torso.Body.GlobalTransform.Basis;
+        Vector3 torsoCenter = _torso.Body.GlobalPosition;
 
         Vector3 topCenter = torsoCenter + Vector3.Up * ShoulderRise;
         Vector3 leftShoulder = topCenter - _torsoBasis.X * _shoulderHalfWidth;
@@ -115,6 +107,7 @@ public partial class BodyRig : Node3D
         Vector3 horizontal = new(_torsoOffset.X, 0f, _torsoOffset.Z);
         horizontal = horizontal.LimitLength(ReachBudgetMax);
         _torsoOffset = new Vector3(horizontal.X, Mathf.Clamp(_torsoOffset.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z);
+        _torso.ReachOffset = _torsoOffset;
     }
 
     private void UpdateYawBasis()
@@ -126,37 +119,9 @@ public partial class BodyRig : Node3D
         _yawBasis = Basis.LookingAt(fwd.Normalized(), Vector3.Up);
     }
 
-    private void UpdateTorsoYaw(Vector3 headPos, float delta)
-    {
-        float hmdYaw = YawOf(-_yawBasis.Z);
-        RigidBody3D leftBody = PhysicsHands.GetBody(_left);
-        RigidBody3D rightBody = PhysicsHands.GetBody(_right);
-        float targetYaw = hmdYaw;
-        if (leftBody != null && rightBody != null)
-        {
-            Vector3 toHands = (leftBody.GlobalPosition + rightBody.GlobalPosition) * 0.5f - headPos;
-            Vector3 horizontal = new(toHands.X, 0f, toHands.Z);
-            if (horizontal.LengthSquared() > 0.0001f)
-            {
-                float handsYaw = YawOf(horizontal.Normalized());
-                float forwardness = Mathf.Clamp(horizontal.Dot(-_yawBasis.Z) / TorsoYawForwardness, 0f, 1f);
-                targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * TorsoYawHandsWeight);
-            }
-        }
-        _torsoYaw = Mathf.LerpAngle(_torsoYaw, targetYaw, 1f - Mathf.Exp(-delta * TorsoYawSmoothRate));
-        Vector3 fwd = new(-Mathf.Sin(_torsoYaw), 0f, -Mathf.Cos(_torsoYaw));
-        _torsoBasis = Basis.LookingAt(fwd, Vector3.Up);
-    }
-
-    private static float YawOf(Vector3 forward)
-    {
-        return Mathf.Atan2(-forward.X, -forward.Z);
-    }
-
     private void Calibrate(float headHeight)
     {
         _calibrated = true;
-        _torsoYaw = YawOf(-_yawBasis.Z);
         float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f);
         _upperLen = 0.5f * armTotal;
         _foreLen = 0.5f * armTotal;

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();
+    }
+}

scripts/BodySpike.cs.uid

@@ -0,0 +1 @@
+uid://bm24msqxk21rx

scripts/PhysicsHands.cs

@@ -66,8 +66,8 @@ public partial class PhysicsHands : Node
             ContinuousCd = true,
             ContactMonitor = true,
             MaxContactsReported = 8,
-            CollisionLayer = 1,
-            CollisionMask = 1,
+            CollisionLayer = 0b1000,  // layer 4 = player hands
+            CollisionMask = 0b1001,   // world + hands; chest (layer 3) excluded both ways
         };
         body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } });
         MeshInstance3D mesh = new() { Mesh = new BoxMesh { Size = HandSize } };

scripts/PhysicsTorso.cs

@@ -0,0 +1,165 @@
+using Godot;
+
+public partial class PhysicsTorso : Node
+{
+    private const float TorsoMass = 30f;
+    private const float MaxForce = 2400f;
+    private const float MaxSpeed = 6f;
+    public const float SnapDistance = 2f;
+    private const float TorsoDrop = 0.42f;
+    private const float TorsoBackOffset = 0.06f;
+    private const float HeadBackOffset = 0.06f;
+    public const float YawHandsWeight = 0.6f;
+    public const float YawForwardness = 0.3f;
+    public const float YawSmoothRate = 12f;
+    private const float OrientKp = 20f;
+    private const float OrientMaxSpin = 10f;
+    private const float OrientMaxAlpha = 150f;
+
+    private static readonly Vector3 TorsoSize = new(0.22f, 0.5f, 0.13f);
+
+    private static readonly Color BodyColor = new(0.4f, 0.45f, 0.55f);
+
+    public Node3D HeadSource;
+    public RigidBody3D HandBodyLeft;
+    public RigidBody3D HandBodyRight;
+    public Vector3 ReachOffset;
+
+    private XRCamera3D _camera;
+    private XRController3D _leftController;
+    private XRController3D _rightController;
+    private RigidBody3D _handL;
+    private RigidBody3D _handR;
+    private RigidBody3D _body;
+
+    private Basis _yawBasis = Basis.Identity;
+    private Basis _torsoBasis = Basis.Identity;
+    private float _torsoYaw;
+    private bool _yawInitialized;
+    private bool _arrived;
+    private MeshInstance3D _mesh;
+
+    public RigidBody3D Body => _body;
+    public Basis TorsoBasis => _body != null ? _body.GlobalTransform.Basis : _torsoBasis;
+    public Vector3 TargetPosition { get; private set; }
+
+    public override void _Ready()
+    {
+        _body = new RigidBody3D
+        {
+            Name = "TorsoBody",
+            Mass = TorsoMass,
+            GravityScale = 0f,
+            ContinuousCd = true,
+            ContactMonitor = true,
+            MaxContactsReported = 8,
+            CollisionLayer = 0b100,  // layer 3 = player body
+            CollisionMask = 0b101,   // world + player body; hands (layer 4) excluded both ways
+        };
+        _body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = TorsoSize } });
+        _mesh = new MeshInstance3D { Mesh = new BoxMesh { Size = TorsoSize }, Visible = false };
+        _mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = BodyColor, Roughness = 0.6f });
+        _body.AddChild(_mesh);
+        AddChild(_body);
+
+        if (HeadSource == null)
+        {
+            _camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
+            _leftController = GetNode<XRController3D>("../XROrigin3D/LeftHand");
+            _rightController = GetNode<XRController3D>("../XROrigin3D/RightHand");
+            _handL = PhysicsHands.GetBody(_leftController);
+            _handR = PhysicsHands.GetBody(_rightController);
+        }
+        GD.Print("PhysicsTorso: chase body spawned");
+    }
+
+    public override void _PhysicsProcess(double delta)
+    {
+        Node3D head = HeadSource != null ? HeadSource : (Node3D)_camera;
+        if (head == null || _body == null)
+            return;
+        float dt = (float)delta;
+
+        _yawBasis = FlatYawBasis(head.GlobalTransform.Basis);
+        Vector3 headPos = head.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
+        UpdateTorsoYaw(headPos, dt);
+        Vector3 target = headPos + _torsoBasis * new Vector3(0f, -TorsoDrop, TorsoBackOffset) + ReachOffset;
+        TargetPosition = target;
+
+        if (!_arrived && _body.GlobalPosition.DistanceTo(target) < 0.3f)
+        {
+            _arrived = true;
+            _mesh.Visible = true;
+        }
+
+        if (_body.GlobalPosition.DistanceTo(target) > SnapDistance)
+        {
+            _body.GlobalTransform = new Transform3D(_torsoBasis, target);
+            _body.LinearVelocity = Vector3.Zero;
+            _body.AngularVelocity = Vector3.Zero;
+            return;
+        }
+
+        Vector3 desiredVelocity = (target - _body.GlobalPosition) / dt;
+        if (desiredVelocity.Length() > MaxSpeed)
+            desiredVelocity = desiredVelocity.Normalized() * MaxSpeed;
+        Vector3 dv = desiredVelocity - _body.LinearVelocity;
+        Vector3 force = dv / dt * TorsoMass;
+        if (force.Length() > MaxForce)
+            force = force.Normalized() * MaxForce;
+        _body.LinearVelocity += force / TorsoMass * dt;
+
+        Quaternion qErr = _torsoBasis.GetRotationQuaternion()
+            * _body.GlobalTransform.Basis.GetRotationQuaternion().Inverse();
+        if (qErr.W < 0f)
+            qErr = new Quaternion(-qErr.X, -qErr.Y, -qErr.Z, -qErr.W);
+        float angle = qErr.GetAngle();
+        Vector3 spinAxis = angle > 1e-5f ? qErr.GetAxis() : Vector3.Up;
+        Vector3 spinDesired = spinAxis * Mathf.Min(angle * OrientKp, OrientMaxSpin);
+
+        Vector3 wErr = spinDesired - _body.AngularVelocity;
+        float maxDeltaW = OrientMaxAlpha * dt;
+        if (wErr.Length() > maxDeltaW)
+            wErr = wErr.Normalized() * maxDeltaW;
+        _body.AngularVelocity += wErr;
+    }
+
+    private void UpdateTorsoYaw(Vector3 headPos, float delta)
+    {
+        float hmdYaw = YawOf(-_yawBasis.Z);
+        float targetYaw = hmdYaw;
+        if (_handL != null && _handR != null)
+        {
+            Vector3 toHands = (_handL.GlobalPosition + _handR.GlobalPosition) * 0.5f - headPos;
+            Vector3 horizontal = new(toHands.X, 0f, toHands.Z);
+            if (horizontal.LengthSquared() > 0.0001f)
+            {
+                float handsYaw = YawOf(horizontal.Normalized());
+                float forwardness = Mathf.Clamp(horizontal.Dot(-_yawBasis.Z) / YawForwardness, 0f, 1f);
+                targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * YawHandsWeight);
+            }
+        }
+        if (!_yawInitialized)
+        {
+            _torsoYaw = hmdYaw;
+            _yawInitialized = true;
+        }
+        _torsoYaw = Mathf.LerpAngle(_torsoYaw, targetYaw, 1f - Mathf.Exp(-delta * YawSmoothRate));
+        Vector3 fwd = new(-Mathf.Sin(_torsoYaw), 0f, -Mathf.Cos(_torsoYaw));
+        _torsoBasis = Basis.LookingAt(fwd, Vector3.Up);
+    }
+
+    private static Basis FlatYawBasis(Basis basis)
+    {
+        Vector3 fwd = -basis.Z;
+        fwd.Y = 0f;
+        if (fwd.LengthSquared() < 0.0001f)
+            return Basis.Identity;
+        return Basis.LookingAt(fwd.Normalized(), Vector3.Up);
+    }
+
+    private static float YawOf(Vector3 forward)
+    {
+        return Mathf.Atan2(-forward.X, -forward.Z);
+    }
+}

scripts/PhysicsTorso.cs.uid

@@ -0,0 +1 @@
+uid://cv6gyg2tf1qsg

scripts/PropShop.cs

@@ -45,6 +45,7 @@ public partial class PropShop : Node3D
     {
         Color wood = new(0.45f, 0.3f, 0.18f);
         StaticBody3D table = new();
+        table.CollisionMask = 0b1101;  // world + player body + hands
         table.AddChild(new CollisionShape3D
         {
             Position = new Vector3(0, 0.725f, -0.6f),
@@ -95,6 +96,7 @@ public partial class PropShop : Node3D
             ContinuousCd = spec.Mass < 0.1f,
             ContactMonitor = true,
             MaxContactsReported = 4,
+            CollisionMask = 0b1101,  // world + player body + hands
         };
         body.AddToGroup("prop");