Arms playtest-2 fixes: flip-proof elbow (angle clamp + rate limit, controller-driven), Stage 2b hand coupling (blocked arm holds the hand back)

- Elbow stored as an angle around the arm axis vs a torso-frame reference: 85% hand-suggested,
  from the controller pose (not the physics palm), clamped +-110 deg, 6 rad/s rate limit
  (flip spike: 1.8 cm/tick max vs 13.8 unclamped)
- PhysicsArms.ConstrainHand: while an arm segment is in contact the hand may lead the
  forearm end by 3 cm; PhysicsHands applies it and suspends stuck-snap while active
- ArmSpike: forearm-rest-over-anvil (owner pose: collider fine, drawn forearm 3.9 cm -> 0),
  flip sweep, bar test now asserts the hand is held back; all six spikes PASS x3

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed 2026-10-02 01:27:29 +03:00
1 parent 464cda2f71
commit 7bc6a866eb
5 files changed
+193 -51

No files matched your search

+5 -4
View File
@@ -275,9 +275,10 @@ device only; the editor has an XR simulator for dev. Trackers/action map: see `x
deferred, chase segments are the Box3D-honest version. Hierarchical budgets (Nimso muscle deferred, chase segments are the Box3D-honest version. Hierarchical budgets (Nimso muscle
model) from a 60 kg body-mass stat: hand 90 N ≺ forearm 180 N ≺ upper arm 360 N ≺ chest model) from a 60 kg body-mass stat: hand 90 N ≺ forearm 180 N ≺ upper arm 360 N ≺ chest
2400 N. What it gives: arms that push props, rest on / get blocked by geometry (no 2400 N. What it gives: arms that push props, rest on / get blocked by geometry (no
tunneling), and draw honestly (BodyRig's elbow = the physical elbow). What it does NOT do tunneling), and draw honestly (BodyRig's elbow = the physical elbow). Playtest fixes (2026-10-02): open-loop torso lean
yet (Stage 2b candidates, owner feel decides): a blocked forearm does not hold the HAND (no chest hunting), controller-driven + clamped/rate-limited elbow (no flips), held-prop
back (hands still chase the controllers independently), and prop/NPC forces on the arm arm exceptions, and Stage 2b HAND COUPLING (a blocked arm holds its hand back, 3 cm slack,
don't flow into the chest. Regression: `scenes/arm_spike.tscn` "ARM: overall PASS"; ghost shows the controller). Still NOT done: prop/NPC forces on the arm don't flow into
the chest. Regression: `scenes/arm_spike.tscn` "ARM: overall PASS";
the full body also runs inside `scenes/collapse_spike.tscn`. Design + prior art: the full body also runs inside `scenes/collapse_spike.tscn`. Design + prior art:
`research/vr-body-physics-hugs.md` §5–7, `research/vr-body-physics-godot.md`. `research/vr-body-physics-hugs.md` §5–7, `research/vr-body-physics-godot.md`.
+25 -8
View File
@@ -77,7 +77,8 @@ TERMINOLOGY (owner-corrected — use precisely):
jointed hand yanks the prop — the grabber's grip break runs first instead, rip at 0.8 m). jointed hand yanks the prop — the grabber's grip break runs first instead, rip at 0.8 m).
Hand ghosts: translucent palm boxes shown at the controller pose when the physical hand lags Hand ghosts: translucent palm boxes shown at the controller pose when the physical hand lags
> 0.1 m — ghosts belong to the HANDS, not props. Static `GetBody(controller)` lookup used by > 0.1 m — ghosts belong to the HANDS, not props. Static `GetBody(controller)` lookup used by
grabber and rig. grabber and rig. The chase target passes through `PhysicsArms.ConstrainHand` first (arm
coupling); the stuck snap is also suspended while that coupling is active.
- **PropGrabber.cs** — grab v5.3: SOLVER-OWNED RIGID WELD via a LOCALLY PATCHED addon (needs - **PropGrabber.cs** — grab v5.3: SOLVER-OWNED RIGID WELD via a LOCALLY PATCHED addon (needs
`research/godot-box3d-weld.md` + the v5.2 history in `research/vr-two-handed-grab.md` — the `research/godot-box3d-weld.md` + the v5.2 history in `research/vr-two-handed-grab.md` — the
pin joint's hidden spring targets IDENTITY and made every earlier iteration a "winch": pin joint's hidden spring targets IDENTITY and made every earlier iteration a "winch":
@@ -145,11 +146,12 @@ TERMINOLOGY (owner-corrected — use precisely):
NO joints (Box3D has no usable 3-DOF shoulder/wrist joint — PinJoint's identity spring, no NO joints (Box3D has no usable 3-DOF shoulder/wrist joint — PinJoint's identity spring, no
Generic6DOF — and AGENTS.md forbids joints between tracked bodies): each segment PD-chases Generic6DOF — and AGENTS.md forbids joints between tracked bodies): each segment PD-chases
its analytic 2-bone IK pose, solved EVERY PHYSICS TICK from the PHYSICAL chest shoulders to its analytic 2-bone IK pose, solved EVERY PHYSICS TICK from the PHYSICAL chest shoulders to
the PHYSICAL hand bodies (IK moved here from BodyRig: soft IK 0.98; elbow picked on its circle, 75% HAND-DRIVEN — the PHYSICAL hand bodies (IK moved here from BodyRig: soft IK 0.98; elbow picked on its circle as an ANGLE around the arm axis relative to a torso-frame
the forearm continues behind the wrist along the aim pose's forward axis, which gives down/back/out reference: 85% of the angle the HAND suggests — the forearm continues behind
outward elbows for wraps/hugs — 25% torso-frame down/back/out; smoothed by bounded-rate the wrist along the CONTROLLER's (aim pose) forward axis, not the physics palm's, so a palm
ROTATION around the arm axis (can't pass through zero and flip); capped at fighting contact can't swing the elbow — clamped to ±110° (never through the inward/up side)
max(shoulder, wrist) + 5 cm height). **Hierarchical and rate-limited to 6 rad/s (no 180° flips; spike: 1.8 cm/tick max vs 13.8 unclamped);
capped at max(shoulder, wrist) + 5 cm height). **Hierarchical
budgets** (Nimso muscle model) from `BodyMassStat` 60 kg: hand 90 N ≺ forearm 180 N ≺ upper budgets** (Nimso muscle model) from `BodyMassStat` 60 kg: hand 90 N ≺ forearm 180 N ≺ upper
arm 360 N ≺ chest 2400 N. Orientation = slewed angular-velocity servo via arm 360 N ≺ chest 2400 N. Orientation = slewed angular-velocity servo via
`BodyMath.RotationError`; segment roll follows the body's current X axis (no flips). `BodyMath.RotationError`; segment roll follows the body's current X axis (no flips).
@@ -160,7 +162,12 @@ TERMINOLOGY (owner-corrected — use precisely):
closed loop (lean from the body's own shoulders) made the chest hunt back and forth — closed loop (lean from the body's own shoulders) made the chest hunt back and forth —
owner-reported, spike-measured 1.1 m/s → 0.019 m/s after the fix. Props held by a hand get owner-reported, spike-measured 1.1 m/s → 0.019 m/s after the fix. Props held by a hand get
a collision exception with that side's arm segments (`SetHeld`, called by PropGrabber) — a collision exception with that side's arm segments (`SetHeld`, called by PropGrabber) —
a 180 N forearm shoving a prop welded to a 90 N hand was a fight. Exposes a 180 N forearm shoving a prop welded to a 90 N hand was a fight. **Stage 2b hand coupling**
(`ConstrainHand`, called by PhysicsHands before chasing): while an arm segment is in
contact, the hand may lead the forearm's end by at most `HandArmSlack` 3 cm — a blocked
forearm holds the hand back and the hand ghost shows the controller. Free arms never limit
the hand. Fixes the owner's "palm behind the anvil, forearm rests on it" clip (drawn
forearm 3.9 cm into the anvil → 0; the collider was always fine at ≤ 6 mm). Exposes
`Shoulder/Elbow/Wrist/ElbowTarget/WristTarget(side)`; `Elbow` = where the two physical `Shoulder/Elbow/Wrist/ElbowTarget/WristTarget(side)`; `Elbow` = where the two physical
segments meet. Spike: `scenes/arm_spike.tscn` (stretch-beyond-reach chest settle, hug elbows out segments meet. Spike: `scenes/arm_spike.tscn` (stretch-beyond-reach chest settle, hug elbows out
0.17 m (old pole: 0.02), held 50 kg anvil stable + forearm out of it, track gap, fast swing, arm pushes a prop 0.17 m (old pole: 0.02), held 50 kg anvil stable + forearm out of it, track gap, fast swing, arm pushes a prop
@@ -278,6 +285,14 @@ Floor StaticBody mask 1|3|4|5 (collision_mask 29).
held anvil) was NOT reproduced headless across 3 anvil heights — the drawn forearm is a held anvil) was NOT reproduced headless across 3 anvil heights — the drawn forearm is a
straight line from the PHYSICAL elbow to the hand wrist, so any elbow below a held object's straight line from the PHYSICAL elbow to the hand wrist, so any elbow below a held object's
near edge can still draw through it; re-check in-headset. near edge can still draw through it; re-check in-headset.
- **Body Stage 2 arms — owner playtest 2 (2026-10-02)**: jitter fixed, elbows "pretty
good". Reported: occasional 180° elbow flip → angle clamp + rate limit, elbow now from the
controller pose; forearm clipping when RESTING the forearm on the anvil with the palm behind
it → measured: collider fine, DRAWN forearm 3.9 cm in (the hand was somewhere a straight
forearm can't connect to) → hand coupling. Owner raised max_physics_steps_per_frame to
12–14: harmless but NOT a quality knob (only catch-up ticks); Box3D's solver quality is
compiled in (`SUB_STEP_COUNT` 4 in `src/spaces/box3d_space_3d.cpp`, contactHertz 30) — the
fork could expose them if contact stiffness ever matters.
- **Body Stage 2 (physical arms) — remaining watch-list.** Things to watch in the first - **Body Stage 2 (physical arms) — remaining watch-list.** Things to watch in the first
playtest: elbow drawn displaced when a segment is blocked (that's the honest physics view), playtest: elbow drawn displaced when a segment is blocked (that's the honest physics view),
arms sweeping props when seated at the table, any stutter at fast swings (spike: ≤ 6 cm arms sweeping props when seated at the table, any stutter at fast swings (spike: ≤ 6 cm
@@ -353,7 +368,9 @@ Floor StaticBody mask 1|3|4|5 (collision_mask 29).
| `PalmGrabOffset` | PropGrabber.cs | ±0.045 m along hand +X (left +, right −) | grab point on the palm (taste choice) | | `PalmGrabOffset` | PropGrabber.cs | ±0.045 m along hand +X (left +, right −) | grab point on the palm (taste choice) |
| Grip/trigger thresholds | several files | 0.6 | polled Float edges | | Grip/trigger thresholds | several files | 0.6 | polled Float edges |
| `SoftIKFactor` | PhysicsArms.cs | 0.98 | shoulder→hand distance clamp before IK solve (lower toward 0.95 if shake persists) | | `SoftIKFactor` | PhysicsArms.cs | 0.98 | shoulder→hand distance clamp before IK solve (lower toward 0.95 if shake persists) |
| `PoleHandWeight` / `PoleSmoothRate` | PhysicsArms.cs | 0.75 / 12 | elbow pole: hand-axis share (forearm continues behind the wrist) vs torso-frame down/back/out; bounded-rate rotation smoothing | | `PoleHandWeight` / `PoleSmoothRate` | PhysicsArms.cs | 0.85 / 12 | share of the hand-suggested elbow angle (from the controller's forward axis); exp smoothing of the angle |
| `PoleMaxAngle` / `PoleMaxSpeed` | PhysicsArms.cs | 110° / 6 rad/s | elbow angle clamp around the down/back/out reference + rate limit (anti-flip) |
| `HandArmSlack` | PhysicsArms.cs | 0.03 m | how far a hand may lead its forearm's end while the arm is in contact |
| `ElbowRiseAllowance` | PhysicsArms.cs | 0.05 m | elbow capped at max(shoulder, wrist) height + this (no chicken wings) | | `ElbowRiseAllowance` | PhysicsArms.cs | 0.05 m | elbow capped at max(shoulder, wrist) height + this (no chicken wings) |
| `TorsoYawHandsWeight` / `TorsoYawForwardness` / `TorsoYawSmoothRate` | PhysicsTorso.cs | 0.6 / 0.3 m / 12 | torso yaw spring: hands blend / forwardness normalizer / exp smoothing (≈200 ms) | | `TorsoYawHandsWeight` / `TorsoYawForwardness` / `TorsoYawSmoothRate` | PhysicsTorso.cs | 0.6 / 0.3 m / 12 | torso yaw spring: hands blend / forwardness normalizer / exp smoothing (≈200 ms) |
| `TorsoMass` / `MaxForce` / `MaxSpeed` | PhysicsTorso.cs | 30 kg / 2400 N / 6 m/s | chest chase budget (force-clamped PD) | | `TorsoMass` / `MaxForce` / `MaxSpeed` | PhysicsTorso.cs | 30 kg / 2400 N / 6 m/s | chest chase budget (force-clamped PD) |
+93 -7
View File
@@ -5,13 +5,16 @@ using Godot;
// Run: godot-mono --headless --fixed-fps 90 --path . scenes/arm_spike.tscn // Run: godot-mono --headless --fixed-fps 90 --path . scenes/arm_spike.tscn
public partial class ArmSpike : Node3D public partial class ArmSpike : Node3D
{ {
private enum Phase { Settle, Stretch, Hug, Hold, Track, Swing, Push, Pin, Teleport, NanGuard, Done } private enum Phase { Settle, Stretch, Hug, Flip, Hold, Rest, Track, Swing, Push, Pin, Teleport, NanGuard, Done }
private const float GapLimit = 0.04f; private const float GapLimit = 0.04f;
private const float StretchChestSpeedLimit = 0.05f; private const float StretchChestSpeedLimit = 0.05f;
private const float StretchLeanRangeLimit = 0.01f; private const float StretchLeanRangeLimit = 0.01f;
private const float HugElbowOutMin = 0.05f; private const float HugElbowOutMin = 0.05f;
private const float HoldPenetrationLimit = 0.01f; private const float HoldPenetrationLimit = 0.01f;
private const float FlipElbowStepLimit = 0.03f;
private const float PinHandLagMin = 0.1f;
private const float RestBodyPenLimit = 0.01f;
private const float HoldAnvilY = 1.25f; // anvil top ≈ shoulder height, like a seated player at the table private const float HoldAnvilY = 1.25f; // anvil top ≈ shoulder height, like a seated player at the table
private const float HoldPitchDeg = 50f; private const float HoldPitchDeg = 50f;
private static readonly Vector3 AnvilSize = new(0.28f, 0.16f, 0.13f); private static readonly Vector3 AnvilSize = new(0.28f, 0.16f, 0.13f);
@@ -31,6 +34,7 @@ public partial class ArmSpike : Node3D
private readonly RigidBody3D[] _hands = new RigidBody3D[2]; private readonly RigidBody3D[] _hands = new RigidBody3D[2];
private readonly Transform3D[] _handTarget = new Transform3D[2]; private readonly Transform3D[] _handTarget = new Transform3D[2];
private readonly Vector3[] _handRest = new Vector3[2]; private readonly Vector3[] _handRest = new Vector3[2];
private readonly Node3D[] _orient = new Node3D[2];
private RigidBody3D _box; private RigidBody3D _box;
private Vector3 _boxStart; private Vector3 _boxStart;
private StaticBody3D _slab; private StaticBody3D _slab;
@@ -46,6 +50,12 @@ public partial class ArmSpike : Node3D
private ConeTwistJoint3D _holdWeld; private ConeTwistJoint3D _holdWeld;
private float _maxHoldPenetration; private float _maxHoldPenetration;
private bool _holdFinite = true; private bool _holdFinite = true;
private float _maxElbowStep;
private Vector3 _lastElbow;
private float _maxPinHandLag;
private float _restBodyPen;
private float _restDrawnPen;
private float _restAnvilMove;
private float _maxStretchChestSpeed; private float _maxStretchChestSpeed;
private Vector3 _stretchOffsetMin = Vector3.One * 9f; private Vector3 _stretchOffsetMin = Vector3.One * 9f;
private Vector3 _stretchOffsetMax = -Vector3.One * 9f; private Vector3 _stretchOffsetMax = -Vector3.One * 9f;
@@ -87,7 +97,12 @@ public partial class ArmSpike : Node3D
AddChild(_hands[side]); AddChild(_hands[side]);
} }
_arms = new PhysicsArms { HeadSource = _head, Torso = _torso, HandLeft = _hands[0], HandRight = _hands[1] }; for (int side = 0; side < 2; side++)
{
_orient[side] = new Node3D();
AddChild(_orient[side]);
}
_arms = new PhysicsArms { HeadSource = _head, Torso = _torso, HandLeft = _hands[0], HandRight = _hands[1], OrientLeft = _orient[0], OrientRight = _orient[1] };
AddChild(_arms); AddChild(_arms);
GD.Print("ARM: arm spike scene ready"); GD.Print("ARM: arm spike scene ready");
} }
@@ -97,7 +112,12 @@ public partial class ArmSpike : Node3D
float dt = (float)delta; float dt = (float)delta;
_phaseT += dt; _phaseT += dt;
for (int side = 0; side < 2; side++) for (int side = 0; side < 2; side++)
PhysicsHands.Drive(_hands[side], _handTarget[side], dt); {
_orient[side].GlobalTransform = _handTarget[side];
Transform3D driveTarget = _handTarget[side];
_arms.ConstrainHand(side, ref driveTarget);
PhysicsHands.Drive(_hands[side], driveTarget, dt);
}
if (!_arms.Calibrated || _arms.UpperArm(0) == null) if (!_arms.Calibrated || _arms.UpperArm(0) == null)
return; return;
@@ -144,6 +164,29 @@ public partial class ArmSpike : Node3D
Record($"hug: elbows out beyond the shoulders (L {outL:F3} m, R {outR:F3} m, limit > {HugElbowOutMin}; elbows L {_arms.Elbow(0)} R {_arms.Elbow(1)})", outMin > HugElbowOutMin); Record($"hug: elbows out beyond the shoulders (L {outL:F3} m, R {outR:F3} m, limit > {HugElbowOutMin}; elbows L {_arms.Elbow(0)} R {_arms.Elbow(1)})", outMin > HugElbowOutMin);
for (int side = 0; side < 2; side++) for (int side = 0; side < 2; side++)
_handTarget[side] = new Transform3D(Basis.Identity, _handRest[side]); _handTarget[side] = new Transform3D(Basis.Identity, _handRest[side]);
Start(Phase.Flip, "flip — right controller's pointing direction sweeps full circles around the arm, elbow must not jump");
}
break;
}
case Phase.Flip:
{
// Hand fixed; its forward axis sweeps a 70° cone around the shoulder→hand
// direction at 1 rev/s, so the hand-suggested elbow circles the arm axis and
// passes the inward/up side every revolution.
Vector3 handPos = new(0.2f, 1.25f, -0.4f);
Vector3 axis = (handPos - _arms.Shoulder(1)).Normalized();
Vector3 perp = axis.Cross(Vector3.Up).Normalized();
Vector3 fwd = (axis * Mathf.Cos(1.22f) + perp.Rotated(axis, Mathf.Tau * _phaseT) * Mathf.Sin(1.22f)).Normalized();
_handTarget[1] = new Transform3D(Basis.LookingAt(fwd, Mathf.Abs(fwd.Y) > 0.95f ? Vector3.Forward : Vector3.Up), handPos);
Vector3 elbow = _arms.ElbowTarget(1);
if (_phaseT > 0.5f)
_maxElbowStep = Mathf.Max(_maxElbowStep, elbow.DistanceTo(_lastElbow));
_lastElbow = elbow;
if (_phaseT > 3.5f)
{
Record($"flip: elbow never jumps (max step {_maxElbowStep:F4} m/tick, limit {FlipElbowStepLimit})", _maxElbowStep < FlipElbowStepLimit);
_handTarget[1] = new Transform3D(Basis.Identity, _handRest[1]);
_maxHoldPenetration = 0f; _maxHoldPenetration = 0f;
Start(Phase.Hold, "hold — right hand grips a 50 kg anvil from the top, forearm must stay out of it"); Start(Phase.Hold, "hold — right hand grips a 50 kg anvil from the top, forearm must stay out of it");
} }
@@ -196,6 +239,48 @@ public partial class ArmSpike : Node3D
_anvil.QueueFree(); _anvil.QueueFree();
_pedestal.QueueFree(); _pedestal.QueueFree();
_handTarget[1] = new Transform3D(Basis.Identity, _handRest[1]); _handTarget[1] = new Transform3D(Basis.Identity, _handRest[1]);
_anvil = null;
Start(Phase.Rest, "rest — palm behind a 50 kg anvil, forearm resting over it (owner playtest 2 pose)");
}
break;
}
case Phase.Rest:
{
// Anvil top below the shoulder; the hand first goes ABOVE the far side, the anvil
// appears, then the hand sinks behind it so the forearm drapes over the top edge.
const float anvilY = 1.25f;
if (_anvil == null && _phaseT > 0.6f)
{
_pedestal = new StaticBody3D { CollisionMask = 0b11101, Position = new Vector3(0.22f, anvilY - 0.13f, -0.30f) };
_pedestal.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.4f, 0.1f, 0.4f) } });
AddChild(_pedestal);
_anvil = new RigidBody3D { Mass = 50f, Position = new Vector3(0.22f, anvilY, -0.30f), CollisionMask = 0b11101 };
_anvil.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = AnvilSize } });
AddChild(_anvil);
}
float sink = Mathf.Clamp((_phaseT - 1f) / 0.8f, 0f, 1f);
Basis down = new Basis(Vector3.Right, Mathf.DegToRad(-20f));
_handTarget[1] = new Transform3D(down, new Vector3(0.22f, 1.45f - 0.17f * sink, -0.47f));
if (_anvil != null && _phaseT > 1.8f)
{
RigidBody3D fore = _arms.Forearm(1);
Vector3 axis = fore.GlobalTransform.Basis.Y * (_arms.ForeLen * 0.5f - 0.02f);
// Collider overlap ≈ forearm center line depth inside the anvil grown by the
// forearm half-thickness (3 cm).
_restBodyPen = Mathf.Max(_restBodyPen, LinePenetration(fore.GlobalPosition - axis, fore.GlobalPosition + axis, _anvil, 0.03f));
Vector3 wrist = _hands[1].GlobalPosition + _hands[1].GlobalTransform.Basis * new Vector3(0f, 0f, 0.06f);
_restDrawnPen = Mathf.Max(_restDrawnPen, LinePenetration(_arms.Elbow(1), wrist, _anvil));
_restAnvilMove = Mathf.Max(_restAnvilMove, _anvil.GlobalPosition.DistanceTo(new Vector3(0.22f, anvilY, -0.30f)));
}
if (_phaseT > 3.5f)
{
GD.Print($"ARM: rest metrics: forearm COLLIDER overlap max {_restBodyPen:F3} m, DRAWN forearm penetration max {_restDrawnPen:F3} m, anvil moved {_restAnvilMove:F3} m, hand {_hands[1].GlobalPosition} elbow {_arms.Elbow(1)}");
Record($"rest: forearm collider stays out of the anvil (overlap {_restBodyPen:F3} m, limit {RestBodyPenLimit})", _restBodyPen < RestBodyPenLimit);
Record($"rest: drawn forearm stays out of the anvil (penetration {_restDrawnPen:F3} m, limit {HoldPenetrationLimit})", _restDrawnPen < HoldPenetrationLimit);
_anvil.QueueFree();
_pedestal.QueueFree();
_handTarget[1] = new Transform3D(Basis.Identity, _handRest[1]);
Start(Phase.Track, "track — hands circle, segments must stay connected, chest undisturbed"); Start(Phase.Track, "track — hands circle, segments must stay connected, chest undisturbed");
} }
break; break;
@@ -298,12 +383,13 @@ public partial class ArmSpike : Node3D
_pinPrevY[side] = c.Y; _pinPrevY[side] = c.Y;
Vector3 foreTarget = (_arms.ElbowTarget(side) + _arms.WristTarget(side)) * 0.5f; Vector3 foreTarget = (_arms.ElbowTarget(side) + _arms.WristTarget(side)) * 0.5f;
_maxPinObstruction = Mathf.Max(_maxPinObstruction, _arms.Forearm(side).GlobalPosition.DistanceTo(foreTarget)); _maxPinObstruction = Mathf.Max(_maxPinObstruction, _arms.Forearm(side).GlobalPosition.DistanceTo(foreTarget));
_maxPinHandLag = Mathf.Max(_maxPinHandLag, _hands[side].GlobalPosition.DistanceTo(_handTarget[side].Origin));
} }
} }
if (_phaseT > 3.5f) if (_phaseT > 3.5f)
{ {
Record($"forearm blocked by bar, no tunneling (deviation max {_maxPinObstruction:F3} m, limit > {PinObstructionMin}; tunneled {_pinTunneled})", Record($"forearm blocked by bar holds the hand back, no tunneling (hand lag max {_maxPinHandLag:F3} m, limit > {PinHandLagMin}; forearm deviation {_maxPinObstruction:F3} m; tunneled {_pinTunneled})",
_maxPinObstruction > PinObstructionMin && !_pinTunneled); _maxPinHandLag > PinHandLagMin && !_pinTunneled);
Record($"pinned under bar: finite and quiet in the last 1 s (segment |v| max {_maxPinSpeed:F3} m/s, limit {PinSpeedLimit})", _pinFinite && _maxPinSpeed < PinSpeedLimit); Record($"pinned under bar: finite and quiet in the last 1 s (segment |v| max {_maxPinSpeed:F3} m/s, limit {PinSpeedLimit})", _pinFinite && _maxPinSpeed < PinSpeedLimit);
_slab.QueueFree(); _slab.QueueFree();
for (int side = 0; side < 2; side++) for (int side = 0; side < 2; side++)
@@ -343,9 +429,9 @@ public partial class ArmSpike : Node3D
} }
// Deepest point of the segment a→b inside the box body (0 if it never enters). // Deepest point of the segment a→b inside the box body (0 if it never enters).
private static float LinePenetration(Vector3 a, Vector3 b, RigidBody3D box) private static float LinePenetration(Vector3 a, Vector3 b, RigidBody3D box, float grow = 0f)
{ {
Vector3 half = AnvilSize * 0.5f; Vector3 half = AnvilSize * 0.5f + Vector3.One * grow;
Transform3D inv = box.GlobalTransform.AffineInverse(); Transform3D inv = box.GlobalTransform.AffineInverse();
float deepest = 0f; float deepest = 0f;
for (int i = 0; i <= 40; i++) for (int i = 0; i <= 40; i++)
+62 -30
View File
@@ -27,8 +27,11 @@ public partial class PhysicsArms : Node
private const float LeanSmoothRate = 6f; private const float LeanSmoothRate = 6f;
private const float WristOffset = 0.06f; private const float WristOffset = 0.06f;
private const float SoftIKFactor = 0.98f; private const float SoftIKFactor = 0.98f;
private const float PoleHandWeight = 0.75f; private const float PoleHandWeight = 0.85f;
private const float PoleSmoothRate = 12f; private const float PoleSmoothRate = 12f;
private const float PoleMaxAngle = 1.92f; // 110 deg either way from down/back/out
private const float PoleMaxSpeed = 6f; // rad/s
public const float HandArmSlack = 0.03f;
private const float ElbowRiseAllowance = 0.05f; private const float ElbowRiseAllowance = 0.05f;
public static float ForearmMaxForce => BodyMassStat * ForearmForcePerKg; public static float ForearmMaxForce => BodyMassStat * ForearmForcePerKg;
@@ -39,6 +42,10 @@ public partial class PhysicsArms : Node
public PhysicsTorso Torso; public PhysicsTorso Torso;
public RigidBody3D HandLeft; public RigidBody3D HandLeft;
public RigidBody3D HandRight; public RigidBody3D HandRight;
// Hand ORIENTATION source for the elbow: the tracked controllers, not the physics palms
// (a palm fighting contact must not swing the elbow). Falls back to the hand body.
public Node3D OrientLeft;
public Node3D OrientRight;
public bool Calibrated { get; private set; } public bool Calibrated { get; private set; }
public float UpperLen { get; private set; } public float UpperLen { get; private set; }
@@ -50,7 +57,8 @@ public partial class PhysicsArms : Node
private readonly Vector3[] _shoulder = new Vector3[2]; private readonly Vector3[] _shoulder = new Vector3[2];
private readonly Vector3[] _elbowTarget = new Vector3[2]; private readonly Vector3[] _elbowTarget = new Vector3[2];
private readonly Vector3[] _wristTarget = new Vector3[2]; private readonly Vector3[] _wristTarget = new Vector3[2];
private readonly Vector3[] _lastPole = new Vector3[2]; private readonly float[] _poleAngle = new float[2];
private readonly Vector3[] _lastRef = new Vector3[2];
private readonly RigidBody3D[] _held = new RigidBody3D[2]; private readonly RigidBody3D[] _held = new RigidBody3D[2];
private Vector3 _torsoOffset; private Vector3 _torsoOffset;
private Basis _torsoBasis = Basis.Identity; private Basis _torsoBasis = Basis.Identity;
@@ -75,6 +83,8 @@ public partial class PhysicsArms : Node
Torso = GetNode<PhysicsTorso>("../PhysicsTorso"); Torso = GetNode<PhysicsTorso>("../PhysicsTorso");
HandLeft = PhysicsHands.GetBody(GetNode<XRController3D>("../XROrigin3D/LeftHand")); HandLeft = PhysicsHands.GetBody(GetNode<XRController3D>("../XROrigin3D/LeftHand"));
HandRight = PhysicsHands.GetBody(GetNode<XRController3D>("../XROrigin3D/RightHand")); HandRight = PhysicsHands.GetBody(GetNode<XRController3D>("../XROrigin3D/RightHand"));
OrientLeft = GetNode<XRController3D>("../XROrigin3D/LeftHand");
OrientRight = GetNode<XRController3D>("../XROrigin3D/RightHand");
} }
} }
@@ -94,8 +104,8 @@ public partial class PhysicsArms : Node
_shoulder[0] = topCenter - _torsoBasis.X * ShoulderHalfWidth; _shoulder[0] = topCenter - _torsoBasis.X * ShoulderHalfWidth;
_shoulder[1] = topCenter + _torsoBasis.X * ShoulderHalfWidth; _shoulder[1] = topCenter + _torsoBasis.X * ShoulderHalfWidth;
SolveArm(0, -_torsoBasis.X, HandLeft, dt); SolveArm(0, -_torsoBasis.X, HandLeft, OrientLeft, dt);
SolveArm(1, _torsoBasis.X, HandRight, dt); SolveArm(1, _torsoBasis.X, HandRight, OrientRight, dt);
UpdateLean(dt); UpdateLean(dt);
for (int side = 0; side < 2; side++) for (int side = 0; side < 2; side++)
@@ -135,6 +145,8 @@ public partial class PhysicsArms : Node
ContinuousCd = true, ContinuousCd = true,
CollisionLayer = 0b10000, // layer 5 = player arms CollisionLayer = 0b10000, // layer 5 = player arms
CollisionMask = 0b1, // world + props only; own chest/hands/arms excluded CollisionMask = 0b1, // world + props only; own chest/hands/arms excluded
ContactMonitor = true,
MaxContactsReported = 4,
}; };
body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } }); body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
AddChild(body); AddChild(body);
@@ -213,6 +225,25 @@ public partial class PhysicsArms : Node
} }
} }
// Stage 2b coupling: while an arm segment is in contact, the hand may lead the forearm's
// end by at most HandArmSlack — a blocked forearm holds the hand back (the hand ghost
// shows the controller). Free arms never limit the hand. PhysicsHands calls this with
// the controller target before chasing; returns true when the limit is active.
public bool ConstrainHand(int side, ref Transform3D target)
{
if (!Calibrated || _fore[side] == null)
return false;
if (_fore[side].GetContactCount() == 0 && _upper[side].GetContactCount() == 0)
return false;
Vector3 wristOffset = target.Basis * new Vector3(0f, 0f, WristOffset);
Vector3 forearmEnd = Wrist(side);
Vector3 lead = target.Origin + wristOffset - forearmEnd;
if (lead.Length() <= HandArmSlack)
return true;
target.Origin = forearmEnd + lead.Normalized() * HandArmSlack - wristOffset;
return true;
}
// Torso lean toward overreaching hands, computed OPEN-LOOP from the head-derived nominal // Torso lean toward overreaching hands, computed OPEN-LOOP from the head-derived nominal
// chest pose — never from the body's own shoulders (that loop made the chest hunt: // chest pose — never from the body's own shoulders (that loop made the chest hunt:
// lean moves shoulders → overreach changes → lean changes). Exact two-arm combination: // lean moves shoulders → overreach changes → lean changes). Exact two-arm combination:
@@ -255,7 +286,7 @@ public partial class PhysicsArms : Node
// "down/back/out" pole fills in where the hand axis says nothing (arm pointing straight // "down/back/out" pole fills in where the hand axis says nothing (arm pointing straight
// along the hand). The pole is smoothed by ROTATING it at a bounded rate around the arm // along the hand). The pole is smoothed by ROTATING it at a bounded rate around the arm
// axis, so it can never pass through zero and flip. // axis, so it can never pass through zero and flip.
private void SolveArm(int index, Vector3 side, RigidBody3D hand, float delta) private void SolveArm(int index, Vector3 side, RigidBody3D hand, Node3D orient, float delta)
{ {
Vector3 shoulder = _shoulder[index]; Vector3 shoulder = _shoulder[index];
Vector3 rawTarget = hand != null ? WristPoint(hand) : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f); Vector3 rawTarget = hand != null ? WristPoint(hand) : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f);
@@ -275,38 +306,39 @@ public partial class PhysicsArms : Node
float radius = Mathf.Sqrt(Mathf.Max(UpperLen * UpperLen - along * along, 0f)); float radius = Mathf.Sqrt(Mathf.Max(UpperLen * UpperLen - along * along, 0f));
Vector3 center = shoulder + dir * along; Vector3 center = shoulder + dir * along;
Vector3 natural = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f; // Reference = torso-frame down/back/out projected off the arm axis. The elbow is
natural -= dir * natural.Dot(dir); // stored as an ANGLE around the arm axis relative to it, clamped to ±110° and
Vector3 pole = natural.LengthSquared() > 1e-8f ? natural.Normalized() * (1f - PoleHandWeight) : Vector3.Zero; // rate-limited — it can never swing through the inward/up side (the 180° flip).
if (hand != null) Vector3 reference = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f;
reference -= dir * reference.Dot(dir);
if (reference.LengthSquared() > 0.06f)
_lastRef[index] = reference.Normalized();
else
{ {
Vector3 handForward = (hand.GlobalTransform.Basis * Vector3.Forward).Normalized(); // Arm pointing along the reference: keep the previous one, re-projected.
reference = _lastRef[index] - dir * _lastRef[index].Dot(dir);
reference = reference.LengthSquared() > 1e-8f ? reference.Normalized() : dir.Cross(_torsoBasis.X).Normalized();
}
Vector3 refN = reference.Normalized();
Vector3 refB = dir.Cross(refN);
float targetAngle = 0f;
Basis handBasis = orient != null ? orient.GlobalTransform.Basis : hand?.GlobalTransform.Basis ?? Basis.Identity;
if (orient != null || hand != null)
{
Vector3 handForward = (handBasis * Vector3.Forward).Normalized();
Vector3 handElbow = wrist - handForward * ForeLen - center; Vector3 handElbow = wrist - handForward * ForeLen - center;
handElbow -= dir * handElbow.Dot(dir); handElbow -= dir * handElbow.Dot(dir);
float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f); float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f);
if (handElbow.LengthSquared() > 1e-8f) if (handElbow.LengthSquared() > 1e-8f)
pole += handElbow.Normalized() * (PoleHandWeight * confidence); targetAngle = Mathf.Atan2(handElbow.Dot(refB), handElbow.Dot(refN)) * PoleHandWeight * confidence;
} }
targetAngle = Mathf.Clamp(targetAngle, -PoleMaxAngle, PoleMaxAngle);
float step = (targetAngle - _poleAngle[index]) * (1f - Mathf.Exp(-delta * PoleSmoothRate));
_poleAngle[index] += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta);
Vector3 pole = refN.Rotated(dir, _poleAngle[index]);
Vector3 last = _lastPole[index]; Vector3 elbow = center + CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius;
last -= dir * last.Dot(dir);
if (last.LengthSquared() < 1e-8f)
last = pole.LengthSquared() > 1e-8f ? pole : natural;
last = last.LengthSquared() > 1e-8f ? last.Normalized() : Vector3.Down;
if (pole.LengthSquared() > 1e-8f)
{
pole = pole.Normalized();
float angle = last.AngleTo(pole);
if (angle > 1e-5f)
{
Vector3 axis = last.Cross(pole);
axis = axis.LengthSquared() > 1e-10f ? axis.Normalized() : dir;
last = last.Rotated(axis, angle * (1f - Mathf.Exp(-delta * PoleSmoothRate)));
}
}
_lastPole[index] = last;
Vector3 elbow = center + CapElbowHeight(last, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius;
_elbowTarget[index] = elbow; _elbowTarget[index] = elbow;
_wristTarget[index] = elbow + (wrist - elbow).Normalized() * ForeLen; _wristTarget[index] = elbow + (wrist - elbow).Normalized() * ForeLen;
} }
+8 -2
View File
@@ -15,6 +15,7 @@ public partial class PhysicsHands : Node
private XRController3D _left; private XRController3D _left;
private XRController3D _right; private XRController3D _right;
private PhysicsArms _arms;
private readonly RigidBody3D[] _bodies = new RigidBody3D[2]; private readonly RigidBody3D[] _bodies = new RigidBody3D[2];
private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2]; private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2];
private readonly float[] _stuckTimer = new float[2]; private readonly float[] _stuckTimer = new float[2];
@@ -24,6 +25,7 @@ public partial class PhysicsHands : Node
_instance = this; _instance = this;
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand"); _left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
_right = GetNode<XRController3D>("../XROrigin3D/RightHand"); _right = GetNode<XRController3D>("../XROrigin3D/RightHand");
_arms = GetNodeOrNull<PhysicsArms>("../PhysicsArms");
_bodies[0] = MakeHand("LeftHandBody", new Color(0.9f, 0.3f, 0.35f)); _bodies[0] = MakeHand("LeftHandBody", new Color(0.9f, 0.3f, 0.35f));
_bodies[1] = MakeHand("RightHandBody", new Color(0.3f, 0.5f, 0.9f)); _bodies[1] = MakeHand("RightHandBody", new Color(0.3f, 0.5f, 0.9f));
_ghosts[0] = MakeGhost(); _ghosts[0] = MakeGhost();
@@ -92,6 +94,8 @@ public partial class PhysicsHands : Node
return; return;
int hand = _left == controller ? 0 : 1; int hand = _left == controller ? 0 : 1;
Vector3 target = controller.GlobalPosition; Vector3 target = controller.GlobalPosition;
Transform3D driveTarget = controller.GlobalTransform;
bool armHeld = _arms != null && _arms.ConstrainHand(hand, ref driveTarget);
bool lagging = body.GlobalPosition.DistanceTo(target) > GhostLagDistance; bool lagging = body.GlobalPosition.DistanceTo(target) > GhostLagDistance;
if (_ghosts[hand] != null) if (_ghosts[hand] != null)
@@ -103,7 +107,9 @@ public partial class PhysicsHands : Node
if (!PropGrabber.IsGrabbing(controller)) if (!PropGrabber.IsGrabbing(controller))
{ {
if (body.GlobalPosition.DistanceTo(target) > StuckDistance) // A hand held back by its own blocked arm is not stuck — snapping it into the
// obstacle would just loop.
if (body.GlobalPosition.DistanceTo(target) > StuckDistance && !armHeld)
_stuckTimer[hand] += delta; _stuckTimer[hand] += delta;
else else
_stuckTimer[hand] = 0f; _stuckTimer[hand] = 0f;
@@ -123,7 +129,7 @@ public partial class PhysicsHands : Node
return; return;
} }
} }
Drive(body, controller.GlobalTransform, delta); Drive(body, driveTarget, delta);
} }
// The hand chase itself — public so headless spikes drive their stand-ins with the real code. // The hand chase itself — public so headless spikes drive their stand-ins with the real code.