Foxyz/VR-Mess
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Body physics Stage 2: physical arm segments (chase-driven upper arm + forearm, hierarchical budgets, layer 5)

Browse Source 
- PhysicsArms: per side 2.0 kg upper arm + 1.2 kg forearm RigidBody3D PD-chasing their
  analytic 2-bone IK pose, solved per physics tick from the physical chest shoulders to the
  physical hands; no joints (Box3D has no usable shoulder/wrist joint; AGENTS forbids joints
  between tracked bodies). Budgets from a 60 kg mass stat: hand 90 < forearm 180 < upper
  arm 360 < chest 2400 N. Owns calibration + reach budget (moved from BodyRig); 1 m snap,
  non-finite reset, BodyMath.RotationError servo.
- Layer 5 = player arms (mask 1); props/table/floor masks include it; no self-collision.
- BodyRig: visuals only, arms drawn shoulder -> physical elbow -> wrist.
- ArmSpike: track gap 0, swing lag <= 6 cm, arm pushes prop with hand clear, forearm blocked
  by bar without tunneling and quiet when pinned, teleport snap, NaN reset — PASS x3.
- CollapseSpike now runs the full body (49/49 x3); all six spikes PASS x3.
- HANDOFF/AGENTS: Stage 2 entry (item 10), layers, tuning table, Stage 2b next steps.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed 2026-10-02 00:04:07 +03:00
1 parent cb518999e5
commit 7589b13aba
11 files changed
+708 -174
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

scripts/BodyRig.cs
+43 -139
View File
@@ -1,52 +1,39 @@
using Godot;
// Placeholder rig VISUALS only: head cube on the HMD, arm boxes drawn from the physical body
// (PhysicsTorso shoulders → PhysicsArms elbow → hand wrist). Arm IK, calibration and the reach
// budget live in PhysicsArms; the torso mesh lives in PhysicsTorso.
public partial class BodyRig : Node3D
{
private const float HeadBackOffset = 0.06f;
private const float HeadSize = 0.15f;
private const float ShoulderRise = 0.25f;
private const float ReachBudgetMax = 0.25f;
private const float ReachVerticalMin = -0.05f;
private const float ReachVerticalMax = 0.12f;
private const float ReachPushSpeed = 0.8f;
private const float ReachDecaySpeed = 0.4f;
private const float ReachDecayFactor = 0.92f;
private const float WristOffset = 0.06f;
private const float SoftIKFactor = 0.98f;
private const float PoleHandWeight = 0.25f;
private const float PoleSmoothRate = 10f;
private XRCamera3D _camera;
private XRController3D _left;
private XRController3D _right;
private PhysicsTorso _torso;
private PhysicsArms _arms;
private MeshInstance3D _headMesh;
private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2];
private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2];
private bool _calibrated;
private float _upperLen;
private float _foreLen;
private float _shoulderHalfWidth;
private Vector3 _torsoOffset = Vector3.Zero;
private bool _sized;
private Basis _yawBasis = Basis.Identity;
private Basis _torsoBasis = Basis.Identity;
private readonly Vector3[] _lastPole = new Vector3[2];
public override void _Ready()
{
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
_right = GetNode<XRController3D>("../XROrigin3D/RightHand");
_torso = GetNode<PhysicsTorso>("../PhysicsTorso");
_arms = GetNode<PhysicsArms>("../PhysicsArms");
_headMesh = MakeMesh(new BoxMesh { Size = new Vector3(HeadSize, HeadSize, HeadSize) }, new Color(0.8f, 0.8f, 0.8f));
_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));
_forearmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));
for (int side = 0; side < 2; side++)
{
_upperArmMeshes[side] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
_forearmMeshes[side] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));
}
SetRigVisible(false);
}
@@ -70,44 +57,37 @@ public partial class BodyRig : Node3D
public override void _Process(double delta)
{
UpdateYawBasis();
float dt = (float)delta;
if (!_calibrated)
{
float headHeight = _camera.GlobalPosition.Y;
if (headHeight <= 0.5f)
return;
Calibrate(headHeight);
}
if (_torso.Body == null)
if (!_arms.Calibrated || _arms.UpperArm(0) == null)
return;
if (!_sized)
{
_sized = true;
for (int side = 0; side < 2; side++)
{
((BoxMesh)_upperArmMeshes[side].Mesh).Size = new Vector3(0.07f, _arms.UpperLen, 0.07f);
((BoxMesh)_forearmMeshes[side].Mesh).Size = new Vector3(0.06f, _arms.ForeLen, 0.06f);
}
SetRigVisible(true);
}
Vector3 headPos = _camera.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
_headMesh.GlobalTransform = new Transform3D(_yawBasis, headPos);
_torsoBasis = _torso.Body.GlobalTransform.Basis;
Vector3 torsoCenter = _torso.Body.GlobalPosition;
PlaceArm(0, PhysicsHands.GetBody(_left));
PlaceArm(1, PhysicsHands.GetBody(_right));
}
Vector3 topCenter = torsoCenter + Vector3.Up * ShoulderRise;
Vector3 leftShoulder = topCenter - _torsoBasis.X * _shoulderHalfWidth;
Vector3 rightShoulder = topCenter + _torsoBasis.X * _shoulderHalfWidth;
Vector3 push = SolveArm(0, leftShoulder, -_torsoBasis.X, PhysicsHands.GetBody(_left), _upperArmMeshes[0], _forearmMeshes[0], dt, out bool decayLeft);
push += SolveArm(1, rightShoulder, _torsoBasis.X, PhysicsHands.GetBody(_right), _upperArmMeshes[1], _forearmMeshes[1], dt, out bool decayRight);
if (push != Vector3.Zero)
{
_torsoOffset += push * (dt * ReachPushSpeed);
}
else if (decayLeft || decayRight)
{
_torsoOffset = _torsoOffset.MoveToward(Vector3.Zero, ReachDecaySpeed * dt);
}
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 PlaceArm(int side, RigidBody3D hand)
{
Vector3 shoulder = _arms.Shoulder(side);
Vector3 elbow = _arms.Elbow(side);
Vector3 wrist = hand != null
? hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset)
: _arms.Wrist(side);
if (!shoulder.IsFinite() || !elbow.IsFinite() || !wrist.IsFinite())
return;
PlaceSegment(_upperArmMeshes[side], shoulder, elbow);
PlaceSegment(_forearmMeshes[side], elbow, wrist);
}
private void UpdateYawBasis()
@@ -119,97 +99,21 @@ public partial class BodyRig : Node3D
_yawBasis = Basis.LookingAt(fwd.Normalized(), Vector3.Up);
}
private void Calibrate(float headHeight)
{
_calibrated = true;
float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f);
_upperLen = 0.5f * armTotal;
_foreLen = 0.5f * armTotal;
_shoulderHalfWidth = 0.16f + headHeight * 0.02f;
((BoxMesh)_upperArmMeshes[0].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f);
((BoxMesh)_upperArmMeshes[1].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f);
((BoxMesh)_forearmMeshes[0].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f);
((BoxMesh)_forearmMeshes[1].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f);
SetRigVisible(true);
GD.Print($"BodyRig: proportions calibrated from head height {headHeight:F2} m " +
$"(arm total {armTotal:F2} m, upper {_upperLen:F2} m, fore {_foreLen:F2} m, shoulder half-width {_shoulderHalfWidth:F2} m)");
}
private Vector3 SolveArm(int index, Vector3 shoulder, Vector3 side, RigidBody3D body, MeshInstance3D upperMesh, MeshInstance3D foreMesh, float delta, out bool decay)
{
Vector3 rawTarget = body != null
? body.GlobalPosition + body.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset)
: shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f);
float reach = _upperLen + _foreLen;
Vector3 shoulderToTarget = rawTarget - shoulder;
if (shoulderToTarget.LengthSquared() < 0.0001f)
shoulderToTarget = _torsoBasis * new Vector3(0f, -0.25f, -0.3f);
float rawDist = shoulderToTarget.Length();
float d = Mathf.Clamp(rawDist, 0.01f, reach * SoftIKFactor);
Vector3 dir = shoulderToTarget / rawDist;
Vector3 clampedTarget = shoulder + dir * d;
Vector3 push = Vector3.Zero;
decay = false;
if (rawDist > reach)
{
push = (rawTarget - clampedTarget).Normalized();
}
else if (rawDist < reach * ReachDecayFactor)
{
decay = true;
}
float cosA = (_upperLen * _upperLen + d * d - _foreLen * _foreLen) / (2f * _upperLen * d);
float angle = Mathf.Acos(Mathf.Clamp(cosA, -1f, 1f));
Vector3 handForward = body != null ? body.GlobalTransform.Basis * Vector3.Forward : Vector3.Zero;
Vector3 pole = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f;
pole -= dir * pole.Dot(dir);
pole -= handForward * PoleHandWeight;
Vector3 poleDir;
if (pole.LengthSquared() < 0.0001f)
poleDir = _lastPole[index] == Vector3.Zero ? Vector3.Down : _lastPole[index];
else
{
poleDir = _lastPole[index].Lerp(pole, 1f - Mathf.Exp(-delta * PoleSmoothRate));
if (poleDir.LengthSquared() < 0.0001f)
poleDir = _lastPole[index];
poleDir = poleDir.Normalized();
_lastPole[index] = poleDir;
}
Vector3 axis = dir.Cross(poleDir);
if (axis.LengthSquared() < 0.0001f)
axis = dir.Cross(Vector3.Up);
if (axis.LengthSquared() < 0.0001f)
axis = Vector3.Right;
axis = axis.Normalized();
Vector3 elbowPos = shoulder + dir.Rotated(axis, angle) * _upperLen;
Vector3 foreEnd = elbowPos + (clampedTarget - elbowPos).Normalized() * _foreLen;
PlaceSegment(upperMesh, shoulder, elbowPos);
PlaceSegment(foreMesh, elbowPos, foreEnd);
return push;
}
// Stretches the unit-length box between two joints so the drawn arm stays connected even
// when a physical segment is held back by contact.
private static void PlaceSegment(MeshInstance3D mesh, Vector3 from, Vector3 to)
{
Vector3 segment = to - from;
if (segment.LengthSquared() < 0.000001f)
float length = segment.Length();
if (length < 0.001f)
return;
Vector3 y = segment.Normalized();
Vector3 y = segment / length;
Vector3 x = Vector3.Up.Cross(y);
if (x.LengthSquared() < 0.0001f)
x = Vector3.Right.Cross(y);
x = x.Normalized();
Vector3 z = x.Cross(y);
mesh.GlobalTransform = new Transform3D(new Basis(x, y, z), (from + to) * 0.5f);
float scale = length / ((BoxMesh)mesh.Mesh).Size.Y;
mesh.GlobalTransform = new Transform3D(new Basis(x, y * scale, z), (from + to) * 0.5f);
}
}