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;