diff --git a/HANDOFF.md b/HANDOFF.md index 686a7a7..9f2b7b7 100644 --- a/HANDOFF.md +++ b/HANDOFF.md @@ -145,16 +145,25 @@ 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, torso-frame pole with - the shoulder→hand axis projected out, hand-forward push, exp-smoothed). **Hierarchical + 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 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). Safety: 1 m snap (teleports), `ResetIfNonFinite`. Also owns calibration (arm lengths + - shoulder width from first head height > 0.5 m; segments spawn then) and the REACH BUDGET - (torso lean toward overreach → `PhysicsTorso.ReachOffset`, hysteresis unchanged). Exposes + shoulder width from first head height > 0.5 m; segments spawn then) and the torso LEAN + toward overreaching hands → `PhysicsTorso.ReachOffset`, computed OPEN-LOOP from + `PhysicsTorso.NominalTarget/TargetBasis` (the head-derived pose without the lean). The old + 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 `Shoulder/Elbow/Wrist/ElbowTarget/WristTarget(side)`; `Elbow` = where the two physical - segments meet. Spike: `scenes/arm_spike.tscn` (track gap, fast swing, arm pushes a prop + 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 with the hand clear, forearm blocked by a bar without tunneling + quiet while pinned, teleport snap, NaN-injection reset, chest undisturbed by own arms). - **BodyRig.cs** — placeholder rig VISUALS only: head cube (HMD-driven) + arm boxes drawn @@ -262,7 +271,14 @@ Floor StaticBody mask 1|3|4|5 (collision_mask 29). static table + two-hand-welded 50 kg anvil) was tested and is stable. Full record: `research/body-stage1-failure-brief.md` (RESOLUTION section). **RULE: never use `Quaternion.GetAngle()/GetAxis()` for servo errors — use `BodyMath.RotationError`.** -- **Body Stage 2 (physical arms) — unvalidated in-headset.** Things to watch in the first +- **Body Stage 2 arms — owner playtest 1 (2026-10-02)**: "works fine for the most part"; + reported forearm clipping through a held anvil, chest jitter when stretching one arm, + elbows mostly pointing down / unnatural. Fixed: open-loop lean (jitter), hand-driven + elbow + height cap, held-prop exceptions. The exact screenshot clip (forearm through the + 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 (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 joint lag at 2 Hz × 0.4 m). Known gaps by design (Stage 2b, feel-driven): a blocked forearm @@ -337,14 +353,14 @@ 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.25 / 10 | elbow pole: hand-forward push / exp smoothing | +| `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 | +| `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) | | `OrientKp` / `OrientMaxSpin` / `OrientMaxAlpha` | PhysicsTorso.cs | 20 / 10 rad/s / 150 rad/s² | chest orientation servo (yaw hold + self-right) | | `SnapDistance` | PhysicsTorso.cs | 2 m | chest teleport snap safety | | `armTotal` | PhysicsArms.cs | clamp(0.46×headHeight, 0.5, 0.72) | total arm length, 50/50 split | -| `ReachPushSpeed` / `ReachDecaySpeed` | PhysicsArms.cs | 0.8 / 0.4 m/s | torso lean toward / back from overreach | -| `ReachDecayFactor` | PhysicsArms.cs | 0.92 | hysteresis band (decay only below 92% of reach) | +| `LeanReachFactor` / `LeanSmoothRate` | PhysicsArms.cs | 0.95 / 6 per s | torso lean starts past 95% reach (open-loop from the nominal chest pose), exp-smoothed ≈170 ms | | `ReachBudgetMax` | PhysicsArms.cs | 0.25 m | horizontal torso offset clamp | | `ReachVerticalMin/Max` | PhysicsArms.cs | −0.05 / +0.12 m | vertical torso offset clamp (stops upward oscillation) | | `HeadBackOffset` | BodyRig.cs / PhysicsTorso.cs | 0.06 m | keeps the head cube out of the view | diff --git a/scripts/ArmSpike.cs b/scripts/ArmSpike.cs index f0603c5..1efe0d9 100644 --- a/scripts/ArmSpike.cs +++ b/scripts/ArmSpike.cs @@ -5,9 +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, Track, Swing, Push, Pin, Teleport, NanGuard, Done } + private enum Phase { Settle, Stretch, Hug, Hold, 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 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); private const float SwingGapLimit = 0.12f; private const float ChestDisturbLimit = 0.01f; private const float PushMinDisplacement = 0.05f; @@ -34,6 +41,15 @@ public partial class ArmSpike : Node3D private float _maxChestErr; private float _minHandBoxDist; private float _maxPinSpeed; + private StaticBody3D _pedestal; + private RigidBody3D _anvil; + private ConeTwistJoint3D _holdWeld; + private float _maxHoldPenetration; + private bool _holdFinite = true; + private float _maxStretchChestSpeed; + private Vector3 _stretchOffsetMin = Vector3.One * 9f; + private Vector3 _stretchOffsetMax = -Vector3.One * 9f; + private readonly RandomNumberGenerator _rng = new() { Seed = 42 }; private float _maxPinObstruction; private readonly float[] _pinPrevY = { float.MinValue, float.MinValue }; private bool _pinTunneled; @@ -89,9 +105,102 @@ public partial class ArmSpike : Node3D { case Phase.Settle: if (_phaseT > 1f) - Start(Phase.Track, "track — hands circle, segments must stay connected, chest undisturbed"); + Start(Phase.Stretch, "stretch — right hand held beyond reach with 1 mm tracker noise, chest must settle (no lean hunting)"); break; + case Phase.Stretch: + { + _handTarget[1].Origin = new Vector3(0.2f, 1.38f, -0.72f) + + new Vector3(_rng.Randfn(), _rng.Randfn(), _rng.Randfn()) * 0.001f; + if (_phaseT > 1.5f) + { + float v = _torso.Body.LinearVelocity.Length(); + _maxStretchChestSpeed = Mathf.Max(_maxStretchChestSpeed, v); + _stretchOffsetMin = _stretchOffsetMin.Min(_torso.ReachOffset); + _stretchOffsetMax = _stretchOffsetMax.Max(_torso.ReachOffset); + } + if (_phaseT > 3f) + { + float range = (_stretchOffsetMax - _stretchOffsetMin).Length(); + Record($"stretch: chest settles (|v| max {_maxStretchChestSpeed:F3} m/s, limit {StretchChestSpeedLimit}; lean range {range:F4} m, limit {StretchLeanRangeLimit}; lean {_torso.ReachOffset})", + _maxStretchChestSpeed < StretchChestSpeedLimit && range < StretchLeanRangeLimit); + _handTarget[1].Origin = _handRest[1]; + Start(Phase.Hug, "hug — hands wrapped inward in front of the chest, elbows must go out to the sides"); + } + break; + } + + case Phase.Hug: + { + // Hands pointing across each other (left points +X, right points -X), as when + // wrapping arms around someone. + _handTarget[0] = new Transform3D(Basis.LookingAt(Vector3.Right, Vector3.Up), new Vector3(-0.12f, 1.25f, -0.45f)); + _handTarget[1] = new Transform3D(Basis.LookingAt(Vector3.Left, Vector3.Up), new Vector3(0.12f, 1.25f, -0.45f)); + if (_phaseT > 1.5f) + { + float outL = -_arms.Elbow(0).X - (-_arms.Shoulder(0).X); + float outR = _arms.Elbow(1).X - _arms.Shoulder(1).X; + float outMin = Mathf.Min(outL, outR); + 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]); + _maxHoldPenetration = 0f; + Start(Phase.Hold, "hold — right hand grips a 50 kg anvil from the top, forearm must stay out of it"); + } + break; + } + + case Phase.Hold: + { + // Far half of the top, fingers pointing forward and steeply down. + Basis down = new Basis(Vector3.Right, Mathf.DegToRad(-HoldPitchDeg)); + _handTarget[1] = new Transform3D(down, new Vector3(0.22f, HoldAnvilY + AnvilSize.Y * 0.5f + 0.08f, -0.44f)); + if (_anvil == null && _phaseT > 0.5f) + { + // 50 kg anvil on a pedestal in front-right; the right hand grips its top, rear + // half, pointing forward and 35 deg down (the playtest screenshot pose). + _pedestal = new StaticBody3D { CollisionMask = 0b11101, Position = new Vector3(0.22f, HoldAnvilY - 0.13f, -0.40f) }; + _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, HoldAnvilY, -0.40f), CollisionMask = 0b11101 }; + _anvil.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = AnvilSize } }); + AddChild(_anvil); + } + if (_holdWeld == null && _phaseT > 0.9f) + { + _holdWeld = new ConeTwistJoint3D + { + ExcludeNodesFromCollision = true, + Position = _hands[1].GlobalPosition, + NodeA = _hands[1].GetPath(), + NodeB = _anvil.GetPath(), + SwingSpan = 0f, + TwistSpan = 0f, + Bias = 0f, + }; + AddChild(_holdWeld); + _arms.SetHeld(1, _anvil); + } + if (_phaseT > 1.3f) + { + _holdFinite &= BodyMath.IsFinite(_arms.Forearm(1)) && BodyMath.IsFinite(_hands[1]) && BodyMath.IsFinite(_anvil); + Vector3 wrist = _hands[1].GlobalPosition + _hands[1].GlobalTransform.Basis * new Vector3(0f, 0f, 0.06f); + _maxHoldPenetration = Mathf.Max(_maxHoldPenetration, LinePenetration(_arms.Elbow(1), wrist, _anvil)); + } + if (_phaseT > 2.8f) + { + Record($"hold: held 50 kg anvil stable, drawn forearm stays out of it (penetration max {_maxHoldPenetration:F3} m, limit {HoldPenetrationLimit}; elbow {_arms.Elbow(1)}; finite {_holdFinite})", + _maxHoldPenetration < HoldPenetrationLimit && _holdFinite); + _arms.SetHeld(1, null); + _holdWeld.QueueFree(); + _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; + } + case Phase.Track: for (int side = 0; side < 2; side++) { @@ -233,6 +342,21 @@ 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) + { + Vector3 half = AnvilSize * 0.5f; + Transform3D inv = box.GlobalTransform.AffineInverse(); + float deepest = 0f; + for (int i = 0; i <= 40; i++) + { + Vector3 local = inv * a.Lerp(b, i / 40f); + float depth = Mathf.Min(half.X - Mathf.Abs(local.X), Mathf.Min(half.Y - Mathf.Abs(local.Y), half.Z - Mathf.Abs(local.Z))); + deepest = Mathf.Max(deepest, depth); + } + return deepest; + } + private float MaxGap() { float gap = 0f; diff --git a/scripts/BodyMath.cs.uid b/scripts/BodyMath.cs.uid new file mode 100644 index 0000000..b2d16dc --- /dev/null +++ b/scripts/BodyMath.cs.uid @@ -0,0 +1 @@ +uid://jcnflxkhghjy diff --git a/scripts/PhysicsArms.cs b/scripts/PhysicsArms.cs index 7b82ef7..918dcfe 100644 --- a/scripts/PhysicsArms.cs +++ b/scripts/PhysicsArms.cs @@ -23,13 +23,13 @@ public partial class PhysicsArms : Node 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 LeanReachFactor = 0.95f; + private const float LeanSmoothRate = 6f; private const float WristOffset = 0.06f; private const float SoftIKFactor = 0.98f; - private const float PoleHandWeight = 0.25f; - private const float PoleSmoothRate = 10f; + private const float PoleHandWeight = 0.75f; + private const float PoleSmoothRate = 12f; + private const float ElbowRiseAllowance = 0.05f; public static float ForearmMaxForce => BodyMassStat * ForearmForcePerKg; public static float UpperArmMaxForce => BodyMassStat * UpperArmForcePerKg; @@ -51,6 +51,7 @@ public partial class PhysicsArms : Node private readonly Vector3[] _elbowTarget = new Vector3[2]; private readonly Vector3[] _wristTarget = new Vector3[2]; private readonly Vector3[] _lastPole = new Vector3[2]; + private readonly RigidBody3D[] _held = new RigidBody3D[2]; private Vector3 _torsoOffset; private Basis _torsoBasis = Basis.Identity; @@ -93,9 +94,9 @@ public partial class PhysicsArms : Node _shoulder[0] = topCenter - _torsoBasis.X * ShoulderHalfWidth; _shoulder[1] = topCenter + _torsoBasis.X * ShoulderHalfWidth; - Vector3 push = SolveArm(0, -_torsoBasis.X, HandLeft, dt, out bool decayLeft); - push += SolveArm(1, _torsoBasis.X, HandRight, dt, out bool decayRight); - UpdateReachBudget(push, decayLeft || decayRight, dt); + SolveArm(0, -_torsoBasis.X, HandLeft, dt); + SolveArm(1, _torsoBasis.X, HandRight, dt); + UpdateLean(dt); for (int side = 0; side < 2; side++) { @@ -117,6 +118,7 @@ public partial class PhysicsArms : Node string name = side == 0 ? "Left" : "Right"; _upper[side] = MakeSegment($"{name}UpperArmBody", UpperArmMass, new Vector3(0.07f, UpperLen - SegmentInset, 0.07f)); _fore[side] = MakeSegment($"{name}ForearmBody", ForearmMass, new Vector3(0.06f, ForeLen - SegmentInset, 0.06f)); + SetHeldExceptions(side, _held[side], true); } GD.Print($"PhysicsArms: 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); " + @@ -186,26 +188,77 @@ public partial class PhysicsArms : Node private static Vector3 SegmentEnd(RigidBody3D body, float length, float sign) => body.GlobalPosition + body.GlobalTransform.Basis.Y * (sign * length * 0.5f); - private void UpdateReachBudget(Vector3 push, bool decay, float dt) + // A prop held by this side's hand must not collide with this side's arm: the forearm + // naturally lies along a held object, and 180 N of forearm shoving a prop welded to a 90 N + // hand is a fight the hand loses. Called by PropGrabber on grab/release. + public void SetHeld(int side, RigidBody3D prop) { - if (push != Vector3.Zero) - _torsoOffset += push * (dt * ReachPushSpeed); - else if (decay) - _torsoOffset = _torsoOffset.MoveToward(Vector3.Zero, ReachDecaySpeed * dt); + if (_held[side] == prop) + return; + SetHeldExceptions(side, _held[side], false); + _held[side] = prop; + SetHeldExceptions(side, prop, true); + } - Vector3 horizontal = new Vector3(_torsoOffset.X, 0f, _torsoOffset.Z).LimitLength(ReachBudgetMax); - _torsoOffset = new Vector3(horizontal.X, Mathf.Clamp(_torsoOffset.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z); + private void SetHeldExceptions(int side, RigidBody3D prop, bool add) + { + if (prop == null || !IsInstanceValid(prop) || _upper[side] == null) + return; + foreach (RigidBody3D segment in new[] { _upper[side], _fore[side] }) + { + if (add) + segment.AddCollisionExceptionWith(prop); + else + segment.RemoveCollisionExceptionWith(prop); + } + } + + // 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: + // the bigger excess first, then whatever the other arm still lacks after that shift. + private void UpdateLean(float dt) + { + Basis basis = Torso.TargetBasis; + Vector3 top = Torso.NominalTarget + Vector3.Up * ShoulderRise; + Vector3 shoulderL = top - basis.X * ShoulderHalfWidth; + Vector3 shoulderR = top + basis.X * ShoulderHalfWidth; + Vector3 excessL = Excess(shoulderL, HandLeft); + Vector3 excessR = Excess(shoulderR, HandRight); + Vector3 lean = excessL.LengthSquared() >= excessR.LengthSquared() + ? excessL + Excess(shoulderR + excessL, HandRight) + : excessR + Excess(shoulderL + excessR, HandLeft); + + Vector3 horizontal = new Vector3(lean.X, 0f, lean.Z).LimitLength(ReachBudgetMax); + lean = new Vector3(horizontal.X, Mathf.Clamp(lean.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z); + _torsoOffset = _torsoOffset.Lerp(lean, 1f - Mathf.Exp(-dt * LeanSmoothRate)); Torso.ReachOffset = _torsoOffset; } - // Analytic 2-bone IK (moved from BodyRig): soft-IK clamp, torso-frame elbow pole with the - // shoulder→hand axis projected out, hand-forward push, exp-smoothed pole. - private Vector3 SolveArm(int index, Vector3 side, RigidBody3D hand, float delta, out bool decay) + private Vector3 Excess(Vector3 shoulder, RigidBody3D hand) + { + if (hand == null) + return Vector3.Zero; + Vector3 toWrist = WristPoint(hand) - shoulder; + float dist = toWrist.Length(); + float limit = (UpperLen + ForeLen) * LeanReachFactor; + return dist > limit ? toWrist * ((dist - limit) / dist) : Vector3.Zero; + } + + private static Vector3 WristPoint(RigidBody3D hand) => + hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset); + + // Analytic 2-bone IK: soft-IK clamp, then pick the elbow on its circle. The elbow + // direction is mostly HAND-DRIVEN — the forearm continues behind the wrist along the + // hand's (aim pose) forward axis, which is where a real forearm is; that gives outward + // elbows for wraps/hugs and keeps the forearm out of held objects. A torso-frame + // "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) { Vector3 shoulder = _shoulder[index]; - Vector3 rawTarget = hand != null - ? hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset) - : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f); + Vector3 rawTarget = hand != null ? WristPoint(hand) : shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f); float reach = UpperLen + ForeLen; Vector3 shoulderToTarget = rawTarget - shoulder; @@ -215,44 +268,64 @@ public partial class PhysicsArms : Node float rawDist = shoulderToTarget.Length(); float d = Mathf.Clamp(rawDist, 0.01f, reach * SoftIKFactor); Vector3 dir = shoulderToTarget / rawDist; - Vector3 clampedTarget = shoulder + dir * d; + Vector3 wrist = shoulder + dir * d; - Vector3 push = Vector3.Zero; - decay = false; - if (rawDist > reach) - push = (rawTarget - clampedTarget).Normalized(); - else if (rawDist < reach * ReachDecayFactor) - decay = true; + // Elbow circle: center on the arm axis, radius from the law of cosines. + float along = (UpperLen * UpperLen - ForeLen * ForeLen + d * d) / (2f * d); + float radius = Mathf.Sqrt(Mathf.Max(UpperLen * UpperLen - along * along, 0f)); + Vector3 center = shoulder + dir * along; - float cosA = (UpperLen * UpperLen + d * d - ForeLen * ForeLen) / (2f * UpperLen * d); - float angle = Mathf.Acos(Mathf.Clamp(cosA, -1f, 1f)); - - Vector3 handForward = hand != null ? hand.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 + 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) { - 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 handForward = (hand.GlobalTransform.Basis * 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); } - 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 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 = shoulder + dir.Rotated(axis, angle) * UpperLen; + Vector3 elbow = center + CapElbowHeight(last, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance) * radius; _elbowTarget[index] = elbow; - _wristTarget[index] = elbow + (clampedTarget - elbow).Normalized() * ForeLen; - return push; + _wristTarget[index] = elbow + (wrist - elbow).Normalized() * ForeLen; + } + + // A real wrist flexes, so following the hand axis literally can throw the elbow above + // the shoulder ("chicken wing"). Keep the elbow no higher than yMax by sliding it down + // its circle, preserving its sideways heading. + private static Vector3 CapElbowHeight(Vector3 pole, Vector3 dir, Vector3 center, float radius, float yMax) + { + if (center.Y + pole.Y * radius <= yMax) + return pole; + Vector3 vertical = Vector3.Up - dir * dir.Y; + if (vertical.LengthSquared() < 1e-6f) + return pole; // arm vertical: the circle is horizontal, height can't change + vertical = vertical.Normalized(); + Vector3 lateral = dir.Cross(vertical); + float cosMax = Mathf.Clamp((yMax - center.Y) / (radius * vertical.Y), -1f, 1f); + float sin = Mathf.Sqrt(1f - cosMax * cosMax); + float sideSign = pole.Dot(lateral) >= 0f ? 1f : -1f; + return (vertical * cosMax + lateral * (sin * sideSign)).Normalized(); } } diff --git a/scripts/PhysicsTorso.cs b/scripts/PhysicsTorso.cs index 8b0e897..245f9d3 100644 --- a/scripts/PhysicsTorso.cs +++ b/scripts/PhysicsTorso.cs @@ -42,6 +42,10 @@ public partial class PhysicsTorso : Node public RigidBody3D Body => _body; public Basis TorsoBasis => _body != null ? _body.GlobalTransform.Basis : _torsoBasis; public Vector3 TargetPosition { get; private set; } + // Head-derived chest target WITHOUT ReachOffset, and its yaw basis — the open-loop frame + // PhysicsArms computes the lean from (reading the body pose instead made the lean hunt). + public Vector3 NominalTarget { get; private set; } + public Basis TargetBasis => _torsoBasis; public override void _Ready() { @@ -83,7 +87,8 @@ public partial class PhysicsTorso : Node _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; + NominalTarget = headPos + _torsoBasis * new Vector3(0f, -TorsoDrop, TorsoBackOffset); + Vector3 target = NominalTarget + ReachOffset; TargetPosition = target; if (!_arrived && _body.GlobalPosition.DistanceTo(target) < 0.3f) diff --git a/scripts/PropGrabber.cs b/scripts/PropGrabber.cs index 6b41503..1f89b26 100644 --- a/scripts/PropGrabber.cs +++ b/scripts/PropGrabber.cs @@ -16,6 +16,7 @@ public partial class PropGrabber : Node private XRController3D _left; private XRController3D _right; + private PhysicsArms _arms; private readonly RigidBody3D[] _held = new RigidBody3D[2]; private readonly ConeTwistJoint3D[] _joints = new ConeTwistJoint3D[2]; @@ -30,6 +31,7 @@ public partial class PropGrabber : Node _instance = this; _left = GetNode("../XROrigin3D/LeftHand"); _right = GetNode("../XROrigin3D/RightHand"); + _arms = GetNodeOrNull("../PhysicsArms"); } public static bool IsGrabbing(XRController3D controller) => @@ -66,6 +68,7 @@ public partial class PropGrabber : Node Vector3 pivot = PalmPoint(handBody, hand); _joints[hand] = MakeWeld(handBody, prop, pivot, RigidWeldBias); _localAnchor[hand] = prop.GlobalTransform.AffineInverse() * pivot; + _arms?.SetHeld(hand, prop); GD.Print($"PropGrabber: grabbed '{prop.Name}' ({prop.Mass:0.###} kg, {(hand == 0 ? "left" : "right")}{(_held[other] == prop ? ", two-handed" : "")})"); } } @@ -182,6 +185,7 @@ public partial class PropGrabber : Node private void DestroyJoint(int hand) { + _arms?.SetHeld(hand, null); if (_joints[hand] != null && IsInstanceValid(_joints[hand])) _joints[hand].QueueFree(); _joints[hand] = null;