From 7bc6a866ebba3fba3db05fef26d081a94e1cd2e6 Mon Sep 17 00:00:00 2001 From: Foxyz Date: Fri, 2 Oct 2026 01:27:29 +0300 Subject: [PATCH] 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 --- AGENTS.md | 9 ++-- HANDOFF.md | 33 +++++++++---- scripts/ArmSpike.cs | 100 +++++++++++++++++++++++++++++++++++++--- scripts/PhysicsArms.cs | 92 ++++++++++++++++++++++++------------ scripts/PhysicsHands.cs | 10 +++- 5 files changed, 193 insertions(+), 51 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 1cfc05c..e3a8afe 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -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 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 - tunneling), and draw honestly (BodyRig's elbow = the physical elbow). What it does NOT do - yet (Stage 2b candidates, owner feel decides): a blocked forearm does not hold the HAND - back (hands still chase the controllers independently), and prop/NPC forces on the arm - don't flow into the chest. Regression: `scenes/arm_spike.tscn` "ARM: overall PASS"; + tunneling), and draw honestly (BodyRig's elbow = the physical elbow). Playtest fixes (2026-10-02): open-loop torso lean + (no chest hunting), controller-driven + clamped/rate-limited elbow (no flips), held-prop + arm exceptions, and Stage 2b HAND COUPLING (a blocked arm holds its hand back, 3 cm slack, + 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: `research/vr-body-physics-hugs.md` §5–7, `research/vr-body-physics-godot.md`. diff --git a/HANDOFF.md b/HANDOFF.md index 9f2b7b7..f52e56e 100644 --- a/HANDOFF.md +++ b/HANDOFF.md @@ -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). 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 - 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 `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": @@ -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 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 - the PHYSICAL hand bodies (IK moved here from BodyRig: soft IK 0.98; elbow picked on its circle, 75% HAND-DRIVEN — - the forearm continues behind the wrist along the aim pose's forward axis, which gives - outward elbows for wraps/hugs — 25% torso-frame down/back/out; smoothed by bounded-rate - ROTATION around the arm axis (can't pass through zero and flip); capped at - max(shoulder, wrist) + 5 cm height). **Hierarchical + 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 + down/back/out reference: 85% of the angle the HAND suggests — the forearm continues behind + the wrist along the CONTROLLER's (aim pose) forward axis, not the physics palm's, so a palm + fighting contact can't swing the elbow — clamped to ±110° (never through the inward/up side) + 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 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). @@ -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 — 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 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 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 @@ -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 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. +- **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 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 @@ -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) | | 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) | -| `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) | | `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) | diff --git a/scripts/ArmSpike.cs b/scripts/ArmSpike.cs index 1efe0d9..30adb86 100644 --- a/scripts/ArmSpike.cs +++ b/scripts/ArmSpike.cs @@ -5,13 +5,16 @@ using Godot; // Run: godot-mono --headless --fixed-fps 90 --path . scenes/arm_spike.tscn 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 StretchChestSpeedLimit = 0.05f; private const float StretchLeanRangeLimit = 0.01f; private const float HugElbowOutMin = 0.05f; 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 HoldPitchDeg = 50f; 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 Transform3D[] _handTarget = new Transform3D[2]; private readonly Vector3[] _handRest = new Vector3[2]; + private readonly Node3D[] _orient = new Node3D[2]; private RigidBody3D _box; private Vector3 _boxStart; private StaticBody3D _slab; @@ -46,6 +50,12 @@ public partial class ArmSpike : Node3D private ConeTwistJoint3D _holdWeld; private float _maxHoldPenetration; 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 Vector3 _stretchOffsetMin = Vector3.One * 9f; private Vector3 _stretchOffsetMax = -Vector3.One * 9f; @@ -87,7 +97,12 @@ public partial class ArmSpike : Node3D 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); GD.Print("ARM: arm spike scene ready"); } @@ -97,7 +112,12 @@ public partial class ArmSpike : Node3D float dt = (float)delta; _phaseT += dt; 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) 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); for (int side = 0; side < 2; 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; 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(); _pedestal.QueueFree(); _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"); } break; @@ -298,12 +383,13 @@ public partial class ArmSpike : Node3D _pinPrevY[side] = c.Y; Vector3 foreTarget = (_arms.ElbowTarget(side) + _arms.WristTarget(side)) * 0.5f; _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) { - Record($"forearm blocked by bar, no tunneling (deviation max {_maxPinObstruction:F3} m, limit > {PinObstructionMin}; tunneled {_pinTunneled})", - _maxPinObstruction > PinObstructionMin && !_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})", + _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); _slab.QueueFree(); 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). - 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(); float deepest = 0f; for (int i = 0; i <= 40; i++) diff --git a/scripts/PhysicsArms.cs b/scripts/PhysicsArms.cs index 918dcfe..79ed97d 100644 --- a/scripts/PhysicsArms.cs +++ b/scripts/PhysicsArms.cs @@ -27,8 +27,11 @@ public partial class PhysicsArms : Node private const float LeanSmoothRate = 6f; private const float WristOffset = 0.06f; 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 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; public static float ForearmMaxForce => BodyMassStat * ForearmForcePerKg; @@ -39,6 +42,10 @@ public partial class PhysicsArms : Node public PhysicsTorso Torso; public RigidBody3D HandLeft; 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 float UpperLen { get; private set; } @@ -50,7 +57,8 @@ public partial class PhysicsArms : Node private readonly Vector3[] _shoulder = new Vector3[2]; private readonly Vector3[] _elbowTarget = 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 Vector3 _torsoOffset; private Basis _torsoBasis = Basis.Identity; @@ -75,6 +83,8 @@ public partial class PhysicsArms : Node Torso = GetNode("../PhysicsTorso"); HandLeft = PhysicsHands.GetBody(GetNode("../XROrigin3D/LeftHand")); HandRight = PhysicsHands.GetBody(GetNode("../XROrigin3D/RightHand")); + OrientLeft = GetNode("../XROrigin3D/LeftHand"); + OrientRight = GetNode("../XROrigin3D/RightHand"); } } @@ -94,8 +104,8 @@ public partial class PhysicsArms : Node _shoulder[0] = topCenter - _torsoBasis.X * ShoulderHalfWidth; _shoulder[1] = topCenter + _torsoBasis.X * ShoulderHalfWidth; - SolveArm(0, -_torsoBasis.X, HandLeft, dt); - SolveArm(1, _torsoBasis.X, HandRight, dt); + SolveArm(0, -_torsoBasis.X, HandLeft, OrientLeft, dt); + SolveArm(1, _torsoBasis.X, HandRight, OrientRight, dt); UpdateLean(dt); for (int side = 0; side < 2; side++) @@ -135,6 +145,8 @@ public partial class PhysicsArms : Node ContinuousCd = true, CollisionLayer = 0b10000, // layer 5 = player arms CollisionMask = 0b1, // world + props only; own chest/hands/arms excluded + ContactMonitor = true, + MaxContactsReported = 4, }; body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } }); 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 // 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: @@ -255,7 +286,7 @@ public partial class PhysicsArms : Node // "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 // 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 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)); Vector3 center = shoulder + dir * along; - Vector3 natural = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f; - natural -= dir * natural.Dot(dir); - Vector3 pole = natural.LengthSquared() > 1e-8f ? natural.Normalized() * (1f - PoleHandWeight) : Vector3.Zero; - if (hand != null) + // Reference = torso-frame down/back/out projected off the arm axis. The elbow is + // stored as an ANGLE around the arm axis relative to it, clamped to ±110° and + // rate-limited — it can never swing through the inward/up side (the 180° flip). + 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; handElbow -= dir * handElbow.Dot(dir); float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f); 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]; - 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; + Vector3 elbow = center + CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius; _elbowTarget[index] = elbow; _wristTarget[index] = elbow + (wrist - elbow).Normalized() * ForeLen; } diff --git a/scripts/PhysicsHands.cs b/scripts/PhysicsHands.cs index f89ecea..a2d56f7 100644 --- a/scripts/PhysicsHands.cs +++ b/scripts/PhysicsHands.cs @@ -15,6 +15,7 @@ public partial class PhysicsHands : Node private XRController3D _left; private XRController3D _right; + private PhysicsArms _arms; private readonly RigidBody3D[] _bodies = new RigidBody3D[2]; private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2]; private readonly float[] _stuckTimer = new float[2]; @@ -24,6 +25,7 @@ public partial class PhysicsHands : Node _instance = this; _left = GetNode("../XROrigin3D/LeftHand"); _right = GetNode("../XROrigin3D/RightHand"); + _arms = GetNodeOrNull("../PhysicsArms"); _bodies[0] = MakeHand("LeftHandBody", new Color(0.9f, 0.3f, 0.35f)); _bodies[1] = MakeHand("RightHandBody", new Color(0.3f, 0.5f, 0.9f)); _ghosts[0] = MakeGhost(); @@ -92,6 +94,8 @@ public partial class PhysicsHands : Node return; int hand = _left == controller ? 0 : 1; Vector3 target = controller.GlobalPosition; + Transform3D driveTarget = controller.GlobalTransform; + bool armHeld = _arms != null && _arms.ConstrainHand(hand, ref driveTarget); bool lagging = body.GlobalPosition.DistanceTo(target) > GhostLagDistance; if (_ghosts[hand] != null) @@ -103,7 +107,9 @@ public partial class PhysicsHands : Node 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; else _stuckTimer[hand] = 0f; @@ -123,7 +129,7 @@ public partial class PhysicsHands : Node 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.