Foxyz/VR-Mess
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Body rebuild step 0: Generic6DOFJoint3D on Box3D (fork patch 2) — ball w/ cone/twist + torque-capped muscle motor, weld, drive

Browse Source 
- addons/godot-box3d/bin/libgodot-box3d.so rebuilt from fork branch generic-6dof-joint
  (weld patch + 6DOF); .vendor/box3d-generic-6dof.patch exported
- SixDofSpike: type, ball pivot/swing, cone+twist limits, muscle holds 2.94 N·m lever at 10 N·m
  cap and fails at 1.5, tracks moving target, drive follows + push-off reaction, weld rigid,
  live flag toggles — PASS x3; all existing spikes PASS x3 on the new binary
- research/godot-box3d-6dof.md (mapping, units, C# muscle recipe); HANDOFF/AGENTS pointers

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed 2026-10-02 05:10:45 +03:00
1 parent 329b7bf143
commit 3a990c6dbe
7 files changed
+896 -2
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

scripts/SixDofSpike.cs
+378
View File
@@ -0,0 +1,378 @@
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;
}
}