508 lines
27 KiB
C#
508 lines
27 KiB
C#
using System.Collections.Generic;
|
|
using Godot;
|
|
|
|
// Headless regression for body-physics Stage 2 (PhysicsArms): chase-driven arm segments.
|
|
// Run: godot-mono --headless --fixed-fps 90 --path . scenes/arm_spike.tscn
|
|
public partial class ArmSpike : Node3D
|
|
{
|
|
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);
|
|
private const float SwingGapLimit = 0.12f;
|
|
private const float ChestDisturbLimit = 0.01f;
|
|
private const float PushMinDisplacement = 0.05f;
|
|
private const float PushHandClearance = 0.08f;
|
|
private const float PinSpeedLimit = 0.3f;
|
|
private const float PinBarY = 1.22f;
|
|
private const float PinObstructionMin = 0.01f;
|
|
private const float TeleportGapLimit = 0.04f;
|
|
private static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);
|
|
|
|
private Node3D _head;
|
|
private PhysicsTorso _torso;
|
|
private PhysicsArms _arms;
|
|
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;
|
|
|
|
private Phase _phase = Phase.Settle;
|
|
private float _phaseT;
|
|
private float _maxGap;
|
|
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 _maxElbowStep;
|
|
private Vector3 _lastElbow;
|
|
private float _maxPinHandLag;
|
|
private float _downSide;
|
|
private int _downSideSwitches;
|
|
private float _restBodyPen;
|
|
private float _restDrawnPen;
|
|
private float _restAnvilMove;
|
|
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;
|
|
private bool _pinFinite = true;
|
|
private readonly List<string> _names = new();
|
|
private readonly List<bool> _ok = new();
|
|
|
|
public override void _Ready()
|
|
{
|
|
StaticBody3D floor = new() { CollisionMask = 0b11101 };
|
|
floor.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(20f, 0.1f, 20f) }, Position = new Vector3(0f, -0.05f, 0f) });
|
|
AddChild(floor);
|
|
|
|
_head = new Node3D { Position = new Vector3(0f, 1.65f, 0f) };
|
|
AddChild(_head);
|
|
_torso = new PhysicsTorso { HeadSource = _head };
|
|
AddChild(_torso);
|
|
_torso.Body.GlobalPosition = new Vector3(0f, 1.23f, 0.12f);
|
|
|
|
for (int side = 0; side < 2; side++)
|
|
{
|
|
float x = side == 0 ? -0.2f : 0.2f;
|
|
_handRest[side] = new Vector3(x, 1.1f, -0.35f);
|
|
_handTarget[side] = new Transform3D(Basis.Identity, _handRest[side]);
|
|
_hands[side] = new RigidBody3D
|
|
{
|
|
Mass = 0.4f,
|
|
GravityScale = 0f,
|
|
ContinuousCd = true,
|
|
Position = _handRest[side],
|
|
CollisionLayer = 0b1000,
|
|
CollisionMask = 0b1001,
|
|
};
|
|
_hands[side].AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } });
|
|
AddChild(_hands[side]);
|
|
}
|
|
|
|
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");
|
|
}
|
|
|
|
public override void _PhysicsProcess(double delta)
|
|
{
|
|
float dt = (float)delta;
|
|
_phaseT += dt;
|
|
for (int side = 0; side < 2; side++)
|
|
{
|
|
_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;
|
|
|
|
switch (_phase)
|
|
{
|
|
case Phase.Settle:
|
|
if (_phaseT > 1f)
|
|
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]);
|
|
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();
|
|
if (_phaseT <= 3.5f)
|
|
_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 && _phaseT <= 5.5f)
|
|
{
|
|
// Controller pointing straight down with tracker-like wobble: the elbow
|
|
// must stay on one side.
|
|
Vector3 wobble = new Vector3(Mathf.Sin(5f * _phaseT), 0f, Mathf.Cos(3.7f * _phaseT)) * 0.5f;
|
|
Vector3 down = (Vector3.Down + wobble).Normalized();
|
|
_handTarget[1] = new Transform3D(Basis.LookingAt(down, Vector3.Forward), handPos);
|
|
float sideNow = Mathf.Sign((_arms.ElbowTarget(1) - _arms.Shoulder(1)).Dot(_torso.Body.GlobalTransform.Basis.X));
|
|
if (_phaseT > 4f)
|
|
{
|
|
if (_downSide == 0f)
|
|
_downSide = sideNow;
|
|
else if (sideNow != _downSide)
|
|
_downSideSwitches++;
|
|
}
|
|
}
|
|
if (_phaseT > 5.5f)
|
|
{
|
|
Record($"point-down: elbow keeps its side under wobble (switches {_downSideSwitches})", _downSideSwitches == 0);
|
|
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");
|
|
}
|
|
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]);
|
|
_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;
|
|
}
|
|
|
|
case Phase.Track:
|
|
for (int side = 0; side < 2; side++)
|
|
{
|
|
float w = side == 0 ? 2.1f : 2.7f;
|
|
_handTarget[side].Origin = _handRest[side] + new Vector3(0.15f * Mathf.Sin(w * _phaseT), 0.15f * Mathf.Cos(w * _phaseT), 0.1f * Mathf.Sin(0.5f * w * _phaseT));
|
|
}
|
|
if (_phaseT > 0.5f)
|
|
{
|
|
_maxGap = Mathf.Max(_maxGap, MaxGap());
|
|
_maxChestErr = Mathf.Max(_maxChestErr, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
|
|
}
|
|
if (_phaseT > 3f)
|
|
{
|
|
Record($"track joint gap max {_maxGap:F3} m (limit {GapLimit})", _maxGap < GapLimit);
|
|
Record($"chest undisturbed by own arms (err max {_maxChestErr:F4} m, limit {ChestDisturbLimit})", _maxChestErr < ChestDisturbLimit);
|
|
Start(Phase.Swing, "swing — fast 0.4 m arm swings at 2 Hz, bounded lag");
|
|
}
|
|
break;
|
|
|
|
case Phase.Swing:
|
|
_handTarget[1].Origin = _handRest[1] + new Vector3(0.2f * Mathf.Sin(12.6f * _phaseT), 0.1f * Mathf.Sin(6.3f * _phaseT), 0f);
|
|
_maxGap = Mathf.Max(_maxGap, MaxGap());
|
|
if (_phaseT > 2f)
|
|
{
|
|
_handTarget[1].Origin = _handRest[1];
|
|
Record($"swing joint gap max {_maxGap:F3} m (limit {SwingGapLimit})", _maxGap < SwingGapLimit);
|
|
Start(Phase.Push, "push — right forearm swings up through a 1 kg box (hand kept clear)");
|
|
// Box just above the forearm's middle; the hand rises 0.3 m ~0.2 m away from it.
|
|
_boxStart = (_arms.ElbowTarget(1) + _arms.WristTarget(1)) * 0.5f + Vector3.Up * 0.11f;
|
|
_box = new RigidBody3D { Mass = 1f, GravityScale = 0f, Position = _boxStart, CollisionMask = 0b11101 };
|
|
_box.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.08f } });
|
|
AddChild(_box);
|
|
_minHandBoxDist = float.MaxValue;
|
|
}
|
|
break;
|
|
|
|
case Phase.Push:
|
|
{
|
|
float k = Mathf.Min(_phaseT / 0.6f, 1f);
|
|
_handTarget[1].Origin = _handRest[1] + Vector3.Up * (0.3f * k);
|
|
_minHandBoxDist = Mathf.Min(_minHandBoxDist, _hands[1].GlobalPosition.DistanceTo(_box.GlobalPosition));
|
|
if (_phaseT > 1.5f)
|
|
{
|
|
float moved = _box.GlobalPosition.DistanceTo(_boxStart);
|
|
Record($"arm segment pushes prop (box moved {moved:F3} m, limit > {PushMinDisplacement}; hand-box min dist {_minHandBoxDist:F3} m, limit > {PushHandClearance})",
|
|
moved > PushMinDisplacement && _minHandBoxDist > PushHandClearance);
|
|
_box.QueueFree();
|
|
_handTarget[1].Origin = _handRest[1];
|
|
// Two short static bars just above the forearms' middles (x clear of the chest).
|
|
// Hands at (±0.2, 1.17, -0.09) put the forearm centers at ~(±0.22, 1.16, 0.09)
|
|
// with the elbows swung back; then the hands rise 0.25 m in front of the bars,
|
|
// so the forearm targets sweep up through them.
|
|
// The bars appear once the forearms have settled in the pin pose (t = 0.8 s).
|
|
_maxPinObstruction = 0f;
|
|
Start(Phase.Pin, "pin — forearms blocked under static bars while the hands rise past them");
|
|
}
|
|
break;
|
|
}
|
|
|
|
case Phase.Pin:
|
|
{
|
|
if (_slab == null && _phaseT >= 0.8f)
|
|
{
|
|
_slab = new StaticBody3D { CollisionMask = 0b11101 };
|
|
foreach (float x in new[] { -0.22f, 0.22f })
|
|
_slab.AddChild(new CollisionShape3D { Position = new Vector3(x, 0f, 0f), Shape = new BoxShape3D { Size = new Vector3(0.14f, 0.04f, 0.04f) } });
|
|
AddChild(_slab);
|
|
_slab.GlobalPosition = new Vector3(0f, PinBarY, 0.09f);
|
|
for (int side = 0; side < 2; side++)
|
|
GD.Print($"ARM: pin start side {side}: forearm center {_arms.Forearm(side).GlobalPosition} (bar y {PinBarY})");
|
|
}
|
|
float rise = Mathf.Clamp((_phaseT - 1f) / 0.5f, 0f, 1f) * 0.25f;
|
|
for (int side = 0; side < 2; side++)
|
|
_handTarget[side].Origin = new Vector3(side == 0 ? -0.2f : 0.2f, 1.17f + rise, -0.09f);
|
|
if (_slab != null)
|
|
{
|
|
for (int side = 0; side < 2; side++)
|
|
{
|
|
RigidBody3D fore = _arms.Forearm(side);
|
|
_pinFinite &= BodyMath.IsFinite(fore) && BodyMath.IsFinite(_arms.UpperArm(side));
|
|
if (_phaseT > 2.5f)
|
|
{
|
|
_maxPinSpeed = Mathf.Max(_maxPinSpeed, fore.LinearVelocity.Length());
|
|
_maxPinSpeed = Mathf.Max(_maxPinSpeed, _arms.UpperArm(side).LinearVelocity.Length());
|
|
}
|
|
// Tunneling = the forearm center rises past the bar while still over it in x
|
|
// (bar half-width 0.07 + forearm half-width 0.03). Sliding off sideways and
|
|
// then rising is honest physics.
|
|
Vector3 c = fore.GlobalPosition;
|
|
bool overBar = Mathf.Abs(Mathf.Abs(c.X) - 0.22f) < 0.1f;
|
|
if (_pinPrevY[side] > float.MinValue && _pinPrevY[side] < PinBarY && c.Y >= PinBarY && overBar)
|
|
_pinTunneled = true;
|
|
if (_pinPrevY[side] < PinBarY && c.Y >= PinBarY)
|
|
GD.Print($"ARM: side {side} forearm cleared the bar at x {c.X:F3} (over bar: {overBar}) t {_phaseT:F2}");
|
|
_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 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++)
|
|
_handRest[side] += new Vector3(3f, 0f, 0f);
|
|
_head.GlobalPosition += new Vector3(3f, 0f, 0f);
|
|
for (int side = 0; side < 2; side++)
|
|
{
|
|
_handTarget[side].Origin = _handRest[side];
|
|
_hands[side].GlobalPosition = _handRest[side];
|
|
_hands[side].LinearVelocity = Vector3.Zero;
|
|
}
|
|
Start(Phase.Teleport, "teleport — head + hands jump 3 m, arms must snap");
|
|
}
|
|
break;
|
|
}
|
|
|
|
case Phase.Teleport:
|
|
if (_phaseT > 0.4f)
|
|
{
|
|
float gap = MaxGap();
|
|
Record($"teleport re-attach gap {gap:F3} m (limit {TeleportGapLimit})", gap < TeleportGapLimit);
|
|
_arms.Forearm(0).AngularVelocity = new Vector3(float.NaN, 0f, 0f);
|
|
Start(Phase.NanGuard, "nan guard — inject NaN into the left forearm, it must reset");
|
|
}
|
|
break;
|
|
|
|
case Phase.NanGuard:
|
|
if (_phaseT > 0.3f)
|
|
{
|
|
bool finite = BodyMath.IsFinite(_arms.Forearm(0));
|
|
float gap = MaxGap();
|
|
Record($"non-finite segment reset (finite {finite}, gap {gap:F3} m)", finite && gap < GapLimit);
|
|
Finish();
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
// 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, float grow = 0f)
|
|
{
|
|
Vector3 half = AnvilSize * 0.5f + Vector3.One * grow;
|
|
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;
|
|
for (int side = 0; side < 2; side++)
|
|
{
|
|
RigidBody3D upper = _arms.UpperArm(side);
|
|
RigidBody3D fore = _arms.Forearm(side);
|
|
Vector3 upperProx = upper.GlobalPosition - upper.GlobalTransform.Basis.Y * (_arms.UpperLen * 0.5f);
|
|
Vector3 upperDist = upper.GlobalPosition + upper.GlobalTransform.Basis.Y * (_arms.UpperLen * 0.5f);
|
|
Vector3 foreProx = fore.GlobalPosition - fore.GlobalTransform.Basis.Y * (_arms.ForeLen * 0.5f);
|
|
gap = Mathf.Max(gap, upperProx.DistanceTo(_arms.Shoulder(side)));
|
|
gap = Mathf.Max(gap, upperDist.DistanceTo(foreProx));
|
|
gap = Mathf.Max(gap, _arms.Wrist(side).DistanceTo(_arms.WristTarget(side)));
|
|
}
|
|
return gap;
|
|
}
|
|
|
|
private void Start(Phase phase, string what)
|
|
{
|
|
_phase = phase;
|
|
_phaseT = 0f;
|
|
_maxGap = 0f;
|
|
GD.Print($"ARM: {what}");
|
|
}
|
|
|
|
private void Record(string name, bool ok)
|
|
{
|
|
_names.Add(name);
|
|
_ok.Add(ok);
|
|
GD.Print($"ARM: {name} → {(ok ? "PASS" : "FAIL")}");
|
|
}
|
|
|
|
private void Finish()
|
|
{
|
|
_phase = Phase.Done;
|
|
bool overall = true;
|
|
foreach (bool ok in _ok)
|
|
overall &= ok;
|
|
GD.Print($"ARM: overall {(overall ? "PASS — physical arms functional on Box3D" : "CHECK — inspect failures above")}");
|
|
GetTree().Quit();
|
|
}
|
|
}
|