VR-Mess/scripts/SixDofSpike.cs

using System;
using System.Collections.Generic;
using Godot;

// Headless regression for the forked addon's Generic6DOFJoint3D → Box3D mapping
// (ball = b3SphericalJoint with cone/twist limits + torque-capped motor; drive = b3MotorJoint;
// all-locked = weld). Run first whenever the addon binary changes.
// godot-mono --headless --fixed-fps 90 --path . scenes/sixdof_spike.tscn
public partial class SixDofSpike : Node3D
{
    private const float SwingLimitDeg = 30f;
    private const float TwistLimitDeg = 10f;
    private const float LimitToleranceDeg = 3f;
    private const float PivotLimit = 0.01f;
    private const float LeverMass = 1f;
    private const float LeverLength = 0.6f;
    private const float StrongMuscle = 10f;
    private const float WeakMuscle = 1.5f;
    private const float MuscleKp = 20f;
    private const float MuscleMaxSpin = 8f;

    private readonly List<(string Name, Action<float> Tick, float Duration, Func<bool> Finish)> _phases = new();
    private int _phase = -1;
    private float _t;
    private bool _allOk = true;
    private Node3D _stage;

    public override void _Ready()
    {
        _phases.Add(("joint type", _ => { }, 0.1f, PhaseType));
        _phases.Add(("ball holds pivot and swings freely", BallTick, 1.5f, BallFinish));
        _phases.Add(("cone + twist limits hold under torque", LimitTick, 1.5f, LimitFinish));
        _phases.Add(("strong muscle holds a 2.9 N·m lever horizontal", t => MuscleTick(t, false), 1.5f, () => MuscleFinish(true)));
        _phases.Add(("weak muscle cannot hold it", t => MuscleTick(t, false), 1.5f, () => MuscleFinish(false)));
        _phases.Add(("muscle tracks a moving target", t => MuscleTick(t, true), 1.8f, MuscleTrackFinish));
        _phases.Add(("drive follows a moving target", DriveTick, 1.5f, DriveFollowFinish));
        _phases.Add(("hand pushing into a wall pushes the anchor body away (reaction)", DriveBlockedTick, 1.5f, DriveBlockedFinish));
        _phases.Add(("all-locked config is rigid", WeldTick, 1.5f, WeldFinish));
        _phases.Add(("live flag toggling stays finite", ToggleTick, 1f, ToggleFinish));
        Next();
    }

    public override void _PhysicsProcess(double delta)
    {
        if (_phase >= _phases.Count)
            return;
        float dt = (float)delta;
        _t += dt;
        _phases[_phase].Tick(dt);
        if (_t >= _phases[_phase].Duration)
        {
            bool ok = _phases[_phase].Finish();
            _allOk &= ok;
            GD.Print($"6DOF: {_phases[_phase].Name} → {(ok ? "PASS" : "FAIL")}");
            Next();
        }
    }

    private void Next()
    {
        _stage?.QueueFree();
        _stage = new Node3D();
        AddChild(_stage);
        _t = 0f;
        _phase++;
        if (_phase >= _phases.Count)
        {
            GD.Print($"6DOF: overall {(_allOk ? "PASS — Generic6DOFJoint3D functional on patched Box3D" : "CHECK — inspect failures above")}");
            GetTree().Quit();
            return;
        }
        Setup(_phase);
    }

    // ---- scene pieces ----
    private StaticBody3D _anchor;
    private RigidBody3D _bodyA;
    private RigidBody3D _box;
    private Generic6DofJoint3D _joint;
    private Basis _frameLocalA;
    private Basis _frameLocalB;
    private Vector3 _pivotLocalB;
    private Vector3 _pivotWorld;
    private float _maxPivotSep;
    private float _maxSwing;
    private float _maxTwist;
    private float _swingRange;
    private float _startAngle;
    private float _maxErrDeg;
    private float _endErrDeg;
    private Vector3 _bodyAStart;
    private bool _finite = true;
    private StaticBody3D _wall;

    private RigidBody3D MakeBox(Vector3 size, float mass, Vector3 pos, float gravity = 1f)
    {
        var body = new RigidBody3D { Mass = mass, GravityScale = gravity, Position = pos };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        _stage.AddChild(body);
        return body;
    }

    private StaticBody3D MakeStatic(Vector3 size, Vector3 pos)
    {
        var body = new StaticBody3D { Position = pos };
        body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = size } });
        _stage.AddChild(body);
        return body;
    }

    // Configure BEFORE AddChild (the node pushes everything to the server on enter-tree).
    private Generic6DofJoint3D MakeJoint(PhysicsBody3D a, PhysicsBody3D b, Vector3 at, Action<Generic6DofJoint3D> configure)
    {
        var joint = new Generic6DofJoint3D { Position = at, NodeA = a.GetPath(), NodeB = b.GetPath(), ExcludeNodesFromCollision = true };
        configure(joint);
        _stage.AddChild(joint);
        _frameLocalA = a.GlobalTransform.Basis.Inverse() * joint.GlobalTransform.Basis;
        _frameLocalB = b.GlobalTransform.Basis.Inverse() * joint.GlobalTransform.Basis;
        _pivotLocalB = b.GlobalTransform.AffineInverse() * at;
        _pivotWorld = at;
        return joint;
    }

    private static void ForAxes(Action<Action<Generic6DofJoint3D.Flag, bool>, Action<Generic6DofJoint3D.Param, float>> body, Generic6DofJoint3D j)
    {
        body((f, v) => { j.SetFlagX(f, v); j.SetFlagY(f, v); j.SetFlagZ(f, v); },
             (p, v) => { j.SetParamX(p, v); j.SetParamY(p, v); j.SetParamZ(p, v); });
    }

    private void Setup(int phase)
    {
        _maxPivotSep = _maxSwing = _maxTwist = _swingRange = _maxErrDeg = _endErrDeg = 0f;
        _finite = true;
        switch (phase)
        {
            case 0:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j));
                break;
            case 1:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j));
                _box.ApplyCentralImpulse(new Vector3(2.5f, 0, 0));
                _startAngle = SwingAngle();
                break;
            case 2:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                // Twist axis = joint X; the box hangs along -Y, so the joint basis is rotated to point X down.
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j =>
                {
                    j.Basis = new Basis(Vector3.Forward, Mathf.Pi / 2f);
                    j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, Mathf.DegToRad(-TwistLimitDeg));
                    j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, Mathf.DegToRad(TwistLimitDeg));
                    foreach (var set in new Action<Generic6DofJoint3D.Param, float>[] { j.SetParamY, j.SetParamZ })
                    {
                        set(Generic6DofJoint3D.Param.AngularLowerLimit, Mathf.DegToRad(-SwingLimitDeg));
                        set(Generic6DofJoint3D.Param.AngularUpperLimit, Mathf.DegToRad(SwingLimitDeg));
                    }
                });
                break;
            case 3:
            case 4:
            case 5:
            {
                // Lever sticking out horizontally along +X from a static shoulder.
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(LeverLength, 0.06f, 0.06f), LeverMass, new Vector3(LeverLength * 0.5f, 3, 0));
                float cap = phase == 4 ? WeakMuscle : StrongMuscle;
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) =>
                {
                    f(Generic6DofJoint3D.Flag.EnableAngularLimit, false);
                    f(Generic6DofJoint3D.Flag.EnableMotor, true);
                    p(Generic6DofJoint3D.Param.AngularMotorForceLimit, cap);
                }, j));
                break;
            }
            case 6:
            case 7:
            {
                _bodyA = MakeBox(new Vector3(0.3f, 0.5f, 0.2f), 10f, new Vector3(0, 1.5f, 0), gravity: 0f);
                _box = MakeBox(new Vector3(0.04f, 0.09f, 0.12f), 0.4f, new Vector3(0.4f, 1.5f, 0), gravity: 0f);
                _joint = MakeJoint(_bodyA, _box, new Vector3(0.4f, 1.5f, 0), j => ForAxes((f, p) =>
                {
                    f(Generic6DofJoint3D.Flag.EnableLinearLimit, false);
                    f(Generic6DofJoint3D.Flag.EnableAngularLimit, false);
                    f(Generic6DofJoint3D.Flag.EnableLinearSpring, true);
                    f(Generic6DofJoint3D.Flag.EnableAngularSpring, true);
                    p(Generic6DofJoint3D.Param.LinearSpringStiffness, 10f);
                    p(Generic6DofJoint3D.Param.LinearSpringDamping, 1f);
                    p(Generic6DofJoint3D.Param.LinearMotorForceLimit, 90f);
                    p(Generic6DofJoint3D.Param.AngularSpringStiffness, 10f);
                    p(Generic6DofJoint3D.Param.AngularSpringDamping, 1f);
                    p(Generic6DofJoint3D.Param.AngularMotorForceLimit, 5f);
                }, j));
                _bodyAStart = _bodyA.GlobalPosition;
                if (phase == 7)
                    _wall = MakeStatic(new Vector3(0.1f, 2f, 2f), new Vector3(0.6f, 1.5f, 0));
                break;
            }
            case 8:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), _ => { });
                break;
            case 9:
                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
                _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j));
                break;
        }
    }

    // ---- measurements ----
    private Quaternion FrameA() => (_anchor != null ? _anchor.GlobalTransform.Basis : _bodyA.GlobalTransform.Basis).GetRotationQuaternion() * _frameLocalA.GetRotationQuaternion();
    private Quaternion FrameB() => _box.GlobalTransform.Basis.GetRotationQuaternion() * _frameLocalB.GetRotationQuaternion();
    private Quaternion Rel() => FrameA().Inverse() * FrameB();

    private float SwingAngle()
    {
        Vector3 xa = FrameA() * Vector3.Right;
        Vector3 xb = FrameB() * Vector3.Right;
        return Mathf.RadToDeg(xa.AngleTo(xb));
    }

    private float TwistAngle()
    {
        Quaternion r = Rel();
        return Mathf.RadToDeg(Mathf.Abs(2f * Mathf.Atan2(r.X, r.W)));
    }

    private float PivotSep() => (_box.GlobalTransform * _pivotLocalB).DistanceTo(_pivotWorld);

    private void TrackFinite() => _finite &= BodyMath.IsFinite(_box) && (_bodyA == null || !IsInstanceValid(_bodyA) || BodyMath.IsFinite(_bodyA));

    // ---- phases ----
    private bool PhaseType()
    {
        PhysicsServer3D.JointType type = PhysicsServer3D.JointGetType(_joint.GetRid());
        GD.Print($"6DOF:   server joint type {type}");
        return type == PhysicsServer3D.JointType.Type6Dof;
    }

    private void BallTick(float dt)
    {
        TrackFinite();
        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
        _swingRange = Mathf.Max(_swingRange, Mathf.Abs(SwingAngle() - _startAngle));
    }

    private bool BallFinish()
    {
        GD.Print($"6DOF:   pivot separation max {_maxPivotSep:F4} m (limit {PivotLimit}), swing range {_swingRange:F1} deg (> 20)");
        return _finite && _maxPivotSep < PivotLimit && _swingRange > 20f;
    }

    private void LimitTick(float dt)
    {
        TrackFinite();
        // Strong torques about all axes: swing pushes against the cone, spin against the twist range.
        _box.ApplyTorque(new Vector3(6f, 3f, 6f) * Mathf.Sign(Mathf.Sin(_t * 4f)));
        _maxSwing = Mathf.Max(_maxSwing, SwingAngle());
        _maxTwist = Mathf.Max(_maxTwist, TwistAngle());
        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
    }

    private bool LimitFinish()
    {
        GD.Print($"6DOF:   swing max {_maxSwing:F1} deg (cone {SwingLimitDeg}), twist max {_maxTwist:F1} deg (limit {TwistLimitDeg}), pivot {_maxPivotSep:F4} m");
        return _finite && _maxSwing < SwingLimitDeg + LimitToleranceDeg && _maxTwist < TwistLimitDeg + LimitToleranceDeg
            && _maxSwing > SwingLimitDeg * 0.6f && _maxPivotSep < PivotLimit;
    }

    private Quaternion _muscleTarget = Quaternion.Identity;

    // Muscle = PD on the relative rotation, written as the motor target velocity (joint-frame-A
    // axes); the motor's torque cap is the strength. BodyMath.RotationError gives the error in
    // frame-A coordinates because both quaternions are expressed relative to frame A.
    private void MuscleTick(float dt, bool moving)
    {
        TrackFinite();
        _muscleTarget = moving
            ? new Quaternion(Vector3.Back, Mathf.DegToRad(60f) * Mathf.Min(_t / 1f, 1f))
            : Quaternion.Identity;
        Vector3 err = BodyMath.RotationError(_muscleTarget, Rel());
        Vector3 w = (err * MuscleKp).LimitLength(MuscleMaxSpin);
        _joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X);
        _joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y);
        _joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z);
        float errDeg = Mathf.RadToDeg(err.Length());
        if (_t > 0.5f)
            _maxErrDeg = Mathf.Max(_maxErrDeg, errDeg);
        _endErrDeg = errDeg;
    }

    private bool MuscleFinish(bool expectHold)
    {
        GD.Print($"6DOF:   lever error after settle max {_maxErrDeg:F1} deg, end {_endErrDeg:F1} deg (gravity torque {LeverMass * 9.81f * LeverLength * 0.5f:F2} N·m)");
        return _finite && (expectHold ? _maxErrDeg < 3f : _maxErrDeg > 20f);
    }

    private bool MuscleTrackFinish()
    {
        GD.Print($"6DOF:   tracking a 60 deg raise: end error {_endErrDeg:F1} deg (limit 3)");
        return _finite && _endErrDeg < 3f;
    }

    private Vector3 DriveTarget => new Vector3(0.4f + 0.5f * Mathf.Min(_t / 1f, 1f), 0, 0);

    private void DriveTick(float dt)
    {
        TrackFinite();
        // Equilibrium points are offsets of the target from the creation pose, in joint frame A.
        Vector3 offset = DriveTarget - new Vector3(0.4f, 0, 0);
        _joint.SetParamX(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.X);
        _joint.SetParamY(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.Y);
        _joint.SetParamZ(Generic6DofJoint3D.Param.LinearSpringEquilibriumPoint, offset.Z);
    }

    private bool DriveFollowFinish()
    {
        Vector3 handRel = _bodyA.GlobalTransform.AffineInverse() * _box.GlobalPosition;
        float err = handRel.DistanceTo(DriveTarget);
        GD.Print($"6DOF:   hand at {handRel} vs target {DriveTarget}: error {err:F3} m (limit 0.03)");
        return _finite && err < 0.03f;
    }

    private float _maxHandX;

    private void DriveBlockedTick(float dt)
    {
        DriveTick(dt);
        _maxHandX = Mathf.Max(_maxHandX, _box.GlobalPosition.X);
    }

    private bool DriveBlockedFinish()
    {
        float moved = _bodyA.GlobalPosition.X - _bodyAStart.X;
        // Hand half-depth 0.02 → its centre can't pass x = 0.53 without penetrating the wall face.
        GD.Print($"6DOF:   hand x max {_maxHandX:F3} (wall face 0.55, hand half-width 0.02), anchor body moved {moved:F3} m (< -0.02 = pushed away)");
        return _finite && _maxHandX < 0.545f && moved < -0.02f;
    }

    private void WeldTick(float dt)
    {
        TrackFinite();
        _box.ApplyTorque(new Vector3(0.5f, 0.5f, 0.5f));
        _box.ApplyCentralForce(new Vector3(5f, 0, 0));
        _maxSwing = Mathf.Max(_maxSwing, Mathf.RadToDeg(Rel().GetAngle()));
        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
    }

    private bool WeldFinish()
    {
        GD.Print($"6DOF:   rotation drift max {_maxSwing:F2} deg (limit 2), pivot {_maxPivotSep:F4} m");
        return _finite && _maxSwing < 2f && _maxPivotSep < PivotLimit;
    }

    private int _toggles;

    private void ToggleTick(float dt)
    {
        TrackFinite();
        _toggles++;
        bool on = _toggles % 2 == 0;
        _joint.SetFlagY(Generic6DofJoint3D.Flag.EnableMotor, on);
        _joint.SetFlagX(Generic6DofJoint3D.Flag.EnableAngularLimit, !on);
        _box.ApplyTorque(new Vector3(1f, 0, 1f));
    }

    private bool ToggleFinish()
    {
        GD.Print($"6DOF:   {_toggles} live flag toggles, finite {_finite}, pivot {PivotSep():F4} m");
        return _finite && PivotSep() < 0.05f;
    }
}