561 lines
26 KiB
C#
561 lines
26 KiB
C#
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; drive + angular limits = b3MotorJoint with the fork's swing-ellipse/twist limits,
|
|
// the shoulder). 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;
|
|
// Limited drive (the shoulder): asymmetric swing bounds about joint Y / Z, twist about X.
|
|
private static readonly float LimLowerY = Mathf.DegToRad(-20f), LimUpperY = Mathf.DegToRad(60f);
|
|
private static readonly float LimLowerZ = Mathf.DegToRad(-40f), LimUpperZ = Mathf.DegToRad(100f);
|
|
private static readonly float LimTwist = Mathf.DegToRad(45f);
|
|
private const float SocketKp = 30f, SocketForce = 2000f;
|
|
|
|
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));
|
|
_phases.Add(("limited drive holds the asymmetric swing ellipse + twist under hard torque", LimitedDriveTorqueTick, 4f, LimitedDriveTorqueFinish));
|
|
_phases.Add(("limited drive: muscle reaches targets inside the range, stops at the bound outside it", LimitedDriveMuscleTick, 6f, LimitedDriveMuscleFinish));
|
|
_phases.Add(("limited drive: the linear socket drive still moves the joint (shrug)", LimitedDriveSocketTick, 1.5f, LimitedDriveSocketFinish));
|
|
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;
|
|
case 10:
|
|
case 11:
|
|
case 12:
|
|
// An upper arm (2 kg, 0.3 m) hanging from a static chest through a shoulder-style DRIVE:
|
|
// linear velocity motor = socket, angular velocity motor = muscle, angular limits on.
|
|
// Joint X points down the arm (twist axis), joint Y = world +X, joint Z = world +Z.
|
|
_anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
|
|
_box = MakeBox(new Vector3(0.08f, 0.3f, 0.08f), 2f, new Vector3(0, 2.85f, 0));
|
|
_joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j =>
|
|
{
|
|
j.Basis = new Basis(Vector3.Forward, Mathf.Pi / 2f);
|
|
ForAxes((f, p) =>
|
|
{
|
|
f(Generic6DofJoint3D.Flag.EnableLinearLimit, false);
|
|
f(Generic6DofJoint3D.Flag.EnableAngularLimit, true);
|
|
f(Generic6DofJoint3D.Flag.EnableLinearMotor, true);
|
|
f(Generic6DofJoint3D.Flag.EnableMotor, true);
|
|
p(Generic6DofJoint3D.Param.LinearMotorForceLimit, SocketForce);
|
|
p(Generic6DofJoint3D.Param.AngularMotorForceLimit, phase == 11 ? 30f : 0.5f);
|
|
}, j);
|
|
j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -LimTwist);
|
|
j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, LimTwist);
|
|
j.SetParamY(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerY);
|
|
j.SetParamY(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperY);
|
|
j.SetParamZ(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerZ);
|
|
j.SetParamZ(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperZ);
|
|
});
|
|
_overshootDeg = -999f;
|
|
_maxTwist = 0f;
|
|
_reach = Vector4.Zero;
|
|
_muscleStage = -1;
|
|
_stageErr.Clear();
|
|
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;
|
|
}
|
|
|
|
// ---- limited drive (shoulder) ----
|
|
private float _overshootDeg;
|
|
private Vector4 _reach; // max +Y, max -Y, max +Z, max -Z swing components reached (deg)
|
|
private int _muscleStage;
|
|
private readonly List<string> _stageErr = new();
|
|
private bool _stagesOk = true;
|
|
|
|
// Swing rotation vector (joint Y/Z components, rad) taking frame A's X onto frame B's X, and the
|
|
// remaining twist about X (rad).
|
|
private void SwingTwist(out Vector2 swing, out float twist)
|
|
{
|
|
Quaternion rel = Rel();
|
|
Vector3 b = rel * Vector3.Right;
|
|
float angle = Vector3.Right.AngleTo(b);
|
|
Vector3 axis = Vector3.Right.Cross(b);
|
|
swing = axis.LengthSquared() > 1e-12f ? new Vector2(axis.Y, axis.Z).Normalized() * angle : Vector2.Zero;
|
|
Quaternion swingQ = axis.LengthSquared() > 1e-12f ? new Quaternion(axis.Normalized(), angle) : Quaternion.Identity;
|
|
Quaternion t = swingQ.Inverse() * rel;
|
|
twist = 2f * Mathf.Atan2(t.X, t.W);
|
|
if (twist > Mathf.Pi) twist -= Mathf.Tau;
|
|
if (twist < -Mathf.Pi) twist += Mathf.Tau;
|
|
}
|
|
|
|
// Radius of the quadrant ellipse in the swing direction (same formula as the fork).
|
|
private static float SwingLimit(Vector2 dir)
|
|
{
|
|
float a = dir.X >= 0f ? LimUpperY : -LimLowerY, b = dir.Y >= 0f ? LimUpperZ : -LimLowerZ;
|
|
float u = dir.X / a, v = dir.Y / b;
|
|
return 1f / Mathf.Sqrt(u * u + v * v);
|
|
}
|
|
|
|
// Socket: linear velocity motor toward the pivot (+ a moving offset), like PlayerRagdoll.ShoulderDrive.
|
|
private void SocketDrive(Vector3 offset)
|
|
{
|
|
Vector3 err = _pivotWorld + offset - _box.GlobalTransform * _pivotLocalB;
|
|
Vector3 v = FrameA().Inverse() * (err * SocketKp);
|
|
_joint.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X);
|
|
_joint.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y);
|
|
_joint.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z);
|
|
}
|
|
|
|
private void Muscle(Quaternion target)
|
|
{
|
|
Vector3 err = BodyMath.RotationError(target, 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);
|
|
}
|
|
|
|
private void TrackLimits()
|
|
{
|
|
TrackFinite();
|
|
SwingTwist(out Vector2 s, out float tw);
|
|
if (s.Length() > Mathf.DegToRad(2f))
|
|
_overshootDeg = Mathf.Max(_overshootDeg, Mathf.RadToDeg(s.Length() - SwingLimit(s.Normalized())));
|
|
_maxTwist = Mathf.Max(_maxTwist, Mathf.RadToDeg(Mathf.Abs(tw)));
|
|
_reach = new Vector4(Mathf.Max(_reach.X, Mathf.RadToDeg(s.X)), Mathf.Max(_reach.Y, -Mathf.RadToDeg(s.X)),
|
|
Mathf.Max(_reach.Z, Mathf.RadToDeg(s.Y)), Mathf.Max(_reach.W, -Mathf.RadToDeg(s.Y)));
|
|
_maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
|
|
}
|
|
|
|
private void LimitedDriveTorqueTick(float dt)
|
|
{
|
|
SocketDrive(Vector3.Zero);
|
|
Muscle(Rel()); // weak (0.5 N·m) muscle holding nothing in particular
|
|
// A hard torque sweeping all swing directions (joint Y/Z) plus an alternating twist torque.
|
|
float phi = _t * 3f;
|
|
Vector3 local = new Vector3(6f * Mathf.Sign(Mathf.Sin(_t * 5f)), 25f * Mathf.Cos(phi), 25f * Mathf.Sin(phi));
|
|
_box.ApplyTorque(FrameA() * local);
|
|
TrackLimits();
|
|
}
|
|
|
|
private bool LimitedDriveTorqueFinish()
|
|
{
|
|
GD.Print($"6DOF: 25 N·m sweep: swing overshoot max {_overshootDeg:F1} deg (limit {LimitToleranceDeg}); reached +Y {_reach.X:F0}/60, -Y {_reach.Y:F0}/20, +Z {_reach.Z:F0}/100, -Z {_reach.W:F0}/40 deg; twist max {_maxTwist:F1} deg (limit 45); socket {_maxPivotSep:F4} m");
|
|
bool reached = _reach.X > 50f && _reach.Y > 14f && _reach.Z > 85f && _reach.W > 32f;
|
|
return _finite && _overshootDeg < LimitToleranceDeg && reached && _maxTwist < Mathf.RadToDeg(LimTwist) + LimitToleranceDeg && _maxPivotSep < PivotLimit;
|
|
}
|
|
|
|
// Targets (swing Y, swing Z, twist, deg): inside, inside, inside, then one beyond the +Y bound.
|
|
private static readonly Vector3[] MuscleTargets = { new(40f, 0f, 0f), new(0f, 80f, 0f), new(-15f, -30f, 30f), new(85f, 0f, 0f) };
|
|
|
|
private static Quaternion SwingTwistQuat(Vector3 deg)
|
|
{
|
|
Vector3 rv = new(0f, Mathf.DegToRad(deg.X), Mathf.DegToRad(deg.Y));
|
|
Quaternion swing = rv.LengthSquared() > 1e-12f ? new Quaternion(rv.Normalized(), rv.Length()) : Quaternion.Identity;
|
|
return swing * new Quaternion(Vector3.Right, Mathf.DegToRad(deg.Z));
|
|
}
|
|
|
|
private void LimitedDriveMuscleTick(float dt)
|
|
{
|
|
SocketDrive(Vector3.Zero);
|
|
int stage = Mathf.Min((int)(_t / 1.5f), MuscleTargets.Length - 1);
|
|
Quaternion target = SwingTwistQuat(MuscleTargets[stage]);
|
|
Muscle(target);
|
|
TrackLimits();
|
|
if (_t >= (stage + 1) * 1.5f - dt * 1.5f && _muscleStage < stage)
|
|
{
|
|
_muscleStage = stage;
|
|
SwingTwist(out Vector2 s, out float tw);
|
|
Vector3 got = new(Mathf.RadToDeg(s.X), Mathf.RadToDeg(s.Y), Mathf.RadToDeg(tw));
|
|
if (stage < 3)
|
|
{
|
|
float err = Mathf.RadToDeg(BodyMath.RotationError(target, Rel()).Length());
|
|
_stageErr.Add($"target {MuscleTargets[stage]} → {got.X:F0},{got.Y:F0},{got.Z:F0} err {err:F1}");
|
|
_stagesOk &= err < 3f;
|
|
}
|
|
else
|
|
{
|
|
_stageErr.Add($"target {MuscleTargets[stage]} (beyond +Y 60) → swing Y {got.X:F1}");
|
|
_stagesOk &= got.X < 60f + LimitToleranceDeg && got.X > 55f;
|
|
}
|
|
}
|
|
}
|
|
|
|
private bool LimitedDriveMuscleFinish()
|
|
{
|
|
GD.Print($"6DOF: 30 N·m muscle: {string.Join("; ", _stageErr)}; overshoot max {_overshootDeg:F1} deg; socket {_maxPivotSep:F4} m");
|
|
return _finite && _stagesOk && _stageErr.Count == MuscleTargets.Length && _overshootDeg < LimitToleranceDeg && _maxPivotSep < PivotLimit;
|
|
}
|
|
|
|
private Vector3 ShrugOffset => new Vector3(0f, 0.05f, -0.03f) * Mathf.Min(_t / 0.5f, 1f);
|
|
|
|
private void LimitedDriveSocketTick(float dt)
|
|
{
|
|
SocketDrive(ShrugOffset);
|
|
Muscle(Quaternion.Identity);
|
|
TrackFinite();
|
|
SwingTwist(out Vector2 s, out _);
|
|
_maxSwing = Mathf.Max(_maxSwing, Mathf.RadToDeg(s.Length()));
|
|
}
|
|
|
|
private bool LimitedDriveSocketFinish()
|
|
{
|
|
float err = (_box.GlobalTransform * _pivotLocalB).DistanceTo(_pivotWorld + ShrugOffset);
|
|
GD.Print($"6DOF: socket moved {ShrugOffset.Length():F3} m: tracking error {err:F4} m (limit 0.005), arm swing max {_maxSwing:F1} deg");
|
|
return _finite && err < 0.005f && _maxSwing < 5f;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|