228 lines
9.7 KiB
C#
228 lines
9.7 KiB
C#
using Godot;
|
|
|
|
// Regression for the patched godot-box3d addon: ConeTwistJoint3D (swing 0, twist 0) maps to
|
|
// a solver-owned Box3D weld joint that locks the two bodies at the relative pose captured at
|
|
// creation. Phase 1 asserts a near-rigid weld under load; phase 2 asserts a soft weld
|
|
// (bias > 0) stretches under sustained extreme force and recovers without exploding.
|
|
public partial class WeldSpike : Node3D
|
|
{
|
|
private const float BodyAMass = 1f;
|
|
private const float BodyBMass = 0.05f;
|
|
private const float RigidWeldBias = 0f;
|
|
private const float SoftWeldBias = 0.5f;
|
|
private const float RigidDuration = 3f;
|
|
private const float SoftDuration = 4f;
|
|
private const float LoadDuration = 1.5f;
|
|
private const float PullDuration = 2.5f;
|
|
private const float RigidTorque = 0.5f;
|
|
private const float RigidForce = 0.5f;
|
|
private const float PullForce = 4f;
|
|
private const float MaxSpeed = 10f;
|
|
private const float RigidRotLimitDeg = 2f;
|
|
private const float RigidSepLimit = 0.01f;
|
|
private const float SoftRotLimitDeg = 90f;
|
|
private const float SoftSepLimit = 0.3f;
|
|
private const float SoftSettleRotLimitDeg = 10f;
|
|
private const float SoftSettleSepLimit = 0.05f;
|
|
|
|
private enum Phase { RigidWeld, SoftWeld }
|
|
|
|
private RigidBody3D _bodyA;
|
|
private RigidBody3D _bodyB;
|
|
private ConeTwistJoint3D _joint;
|
|
private Phase _phase = Phase.RigidWeld;
|
|
private float _elapsed;
|
|
private float _phaseStart;
|
|
private Quaternion _relRotation0;
|
|
private Vector3 _localAnchorA;
|
|
private Vector3 _localAnchorB;
|
|
private float _maxRelRot;
|
|
private float _maxSep;
|
|
private float _maxSpeed;
|
|
private bool _typeOk;
|
|
private bool _rigidRotOk;
|
|
private bool _rigidSepOk;
|
|
private bool _rigidSpeedOk;
|
|
private bool _softStretchOk;
|
|
private bool _softSpeedOk;
|
|
private bool _softSettleOk;
|
|
private int _lastLogStep;
|
|
|
|
public override void _Ready()
|
|
{
|
|
_bodyA = new RigidBody3D
|
|
{
|
|
Position = new Vector3(-0.2f, 2f, 0f),
|
|
Mass = BodyAMass,
|
|
GravityScale = 0f,
|
|
ContinuousCd = true,
|
|
};
|
|
_bodyA.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.4f } });
|
|
AddChild(_bodyA);
|
|
|
|
_bodyB = new RigidBody3D
|
|
{
|
|
Position = new Vector3(0.25f, 2f, 0f),
|
|
Mass = BodyBMass,
|
|
GravityScale = 0f,
|
|
ContinuousCd = true,
|
|
};
|
|
_bodyB.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.1f } });
|
|
AddChild(_bodyB);
|
|
|
|
Weld(RigidWeldBias);
|
|
_phaseStart = _elapsed;
|
|
GD.Print($"WELD: rigid weld created at t={_elapsed:F2} s, server joint type = {PhysicsServer3D.JointGetType(_joint.GetRid())} (expect {PhysicsServer3D.JointType.ConeTwist})");
|
|
_typeOk = PhysicsServer3D.JointGetType(_joint.GetRid()) == PhysicsServer3D.JointType.ConeTwist;
|
|
GD.Print("WELD: weld spike scene ready");
|
|
}
|
|
|
|
private void Weld(float bias)
|
|
{
|
|
Vector3 pivot = (_bodyA.GlobalPosition + _bodyB.GlobalPosition) * 0.5f;
|
|
_relRotation0 = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
|
* _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
|
|
_localAnchorA = _bodyA.GlobalTransform.AffineInverse() * pivot;
|
|
_localAnchorB = _bodyB.GlobalTransform.AffineInverse() * pivot;
|
|
_maxRelRot = 0f;
|
|
_maxSep = 0f;
|
|
_maxSpeed = 0f;
|
|
_joint = new ConeTwistJoint3D
|
|
{
|
|
ExcludeNodesFromCollision = true,
|
|
Position = pivot,
|
|
NodeA = _bodyA.GetPath(),
|
|
NodeB = _bodyB.GetPath(),
|
|
SwingSpan = 0f,
|
|
TwistSpan = 0f,
|
|
Bias = bias,
|
|
};
|
|
AddChild(_joint);
|
|
}
|
|
|
|
public override void _PhysicsProcess(double delta)
|
|
{
|
|
float phaseT = _elapsed - _phaseStart;
|
|
_elapsed += (float)delta;
|
|
|
|
switch (_phase)
|
|
{
|
|
case Phase.RigidWeld:
|
|
{
|
|
if (phaseT < LoadDuration)
|
|
{
|
|
_bodyA.ApplyTorque(new Vector3(0f, RigidTorque, 0f));
|
|
_bodyB.ApplyCentralForce(new Vector3(RigidForce, 0f, 0f));
|
|
}
|
|
Track();
|
|
LogBucket(phaseT);
|
|
if (phaseT >= RigidDuration)
|
|
FinishRigid();
|
|
break;
|
|
}
|
|
case Phase.SoftWeld:
|
|
{
|
|
if (phaseT < PullDuration)
|
|
{
|
|
_bodyA.ApplyCentralForce(new Vector3(-PullForce, 0f, 0f));
|
|
_bodyB.ApplyCentralForce(new Vector3(PullForce, 0f, 0f));
|
|
}
|
|
Track();
|
|
LogBucket(phaseT);
|
|
if (phaseT >= SoftDuration)
|
|
Finish();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private static float DevAngle(Quaternion relNow, Quaternion rel0)
|
|
{
|
|
Quaternion dev = relNow * rel0.Inverse();
|
|
if (dev.W < 0f)
|
|
dev = new Quaternion(-dev.X, -dev.Y, -dev.Z, -dev.W);
|
|
// GetAngle() NaNs when w exceeds 1 by a float epsilon; clamp like GrabSpike does
|
|
if (float.IsNaN(dev.W))
|
|
return float.NaN;
|
|
return 2f * Mathf.Acos(Mathf.Clamp(Mathf.Abs(dev.W), 0f, 1f));
|
|
}
|
|
|
|
private void Track()
|
|
{
|
|
Quaternion relNow = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
|
* _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
|
|
float rotDev = DevAngle(relNow, _relRotation0);
|
|
if (float.IsNaN(rotDev))
|
|
{
|
|
GD.Print("WELD: FAIL — NaN rotation error (explosion guard)");
|
|
GetTree().Quit();
|
|
return;
|
|
}
|
|
_maxRelRot = Mathf.Max(_maxRelRot, rotDev);
|
|
float sep = (_bodyA.GlobalTransform * _localAnchorA).DistanceTo(_bodyB.GlobalTransform * _localAnchorB);
|
|
_maxSep = Mathf.Max(_maxSep, sep);
|
|
float speed = Mathf.Max(_bodyA.LinearVelocity.Length(), _bodyB.LinearVelocity.Length());
|
|
if (float.IsNaN(speed) || speed > MaxSpeed)
|
|
{
|
|
GD.Print($"WELD: FAIL — body speed {speed} m/s exceeds {MaxSpeed} (explosion guard)");
|
|
GetTree().Quit();
|
|
return;
|
|
}
|
|
_maxSpeed = Mathf.Max(_maxSpeed, speed);
|
|
}
|
|
|
|
private void LogBucket(float phaseT)
|
|
{
|
|
int step = (int)(phaseT / 0.5f);
|
|
if (step == _lastLogStep)
|
|
return;
|
|
_lastLogStep = step;
|
|
Quaternion relNow = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
|
* _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
|
|
float rotDev = DevAngle(relNow, _relRotation0);
|
|
float sep = (_bodyA.GlobalTransform * _localAnchorA).DistanceTo(_bodyB.GlobalTransform * _localAnchorB);
|
|
GD.Print($"WELD: t={_elapsed:F2} phase={_phase} rotDev={Mathf.RadToDeg(rotDev):F2} deg sep={sep:F4} m speedA={_bodyA.LinearVelocity.Length():F2} speedB={_bodyB.LinearVelocity.Length():F2}");
|
|
}
|
|
|
|
private void FinishRigid()
|
|
{
|
|
_rigidRotOk = _maxRelRot < Mathf.DegToRad(RigidRotLimitDeg);
|
|
_rigidSepOk = _maxSep < RigidSepLimit;
|
|
_rigidSpeedOk = _maxSpeed < MaxSpeed;
|
|
GD.Print($"WELD: rigid hold — max rot dev {Mathf.RadToDeg(_maxRelRot):F2} deg (limit {RigidRotLimitDeg}) → {(_rigidRotOk ? "PASS" : "FAIL")}");
|
|
GD.Print($"WELD: rigid anchors — max sep {_maxSep:F4} m (limit {RigidSepLimit}) → {(_rigidSepOk ? "PASS" : "FAIL")}");
|
|
GD.Print($"WELD: rigid speeds — max {_maxSpeed:F2} m/s (limit {MaxSpeed}) → {(_rigidSpeedOk ? "PASS" : "FAIL")}");
|
|
|
|
// fresh-grab conditions: stop the coasting bodies before re-welding
|
|
_bodyA.LinearVelocity = Vector3.Zero;
|
|
_bodyA.AngularVelocity = Vector3.Zero;
|
|
_bodyB.LinearVelocity = Vector3.Zero;
|
|
_bodyB.AngularVelocity = Vector3.Zero;
|
|
_joint.QueueFree();
|
|
_joint = null;
|
|
_phase = Phase.SoftWeld;
|
|
_phaseStart = _elapsed;
|
|
_lastLogStep = 0;
|
|
Weld(SoftWeldBias);
|
|
GD.Print($"WELD: soft weld re-created at current pose, bias {SoftWeldBias} (weld hertz {SoftWeldBias * 30f:F0}), sustained ±{PullForce} N pull starting");
|
|
}
|
|
|
|
private void Finish()
|
|
{
|
|
_softStretchOk = _maxRelRot < Mathf.DegToRad(SoftRotLimitDeg) && _maxSep < SoftSepLimit;
|
|
_softSpeedOk = _maxSpeed < MaxSpeed;
|
|
// settle: assert the stretch has recovered by the end
|
|
Quaternion relNow = _bodyA.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
|
* _bodyB.GlobalTransform.Basis.GetRotationQuaternion();
|
|
float settleRot = DevAngle(relNow, _relRotation0);
|
|
float settleSep = (_bodyA.GlobalTransform * _localAnchorA).DistanceTo(_bodyB.GlobalTransform * _localAnchorB);
|
|
_softSettleOk = settleRot < Mathf.DegToRad(SoftSettleRotLimitDeg) && settleSep < SoftSettleSepLimit;
|
|
GD.Print($"WELD: soft excursion bounded — max rot dev {Mathf.RadToDeg(_maxRelRot):F2} deg (limit {SoftRotLimitDeg}), max sep {_maxSep:F4} m (limit {SoftSepLimit}) → {(_softStretchOk ? "PASS" : "FAIL")}");
|
|
GD.Print($"WELD: soft speeds — max {_maxSpeed:F2} m/s (limit {MaxSpeed}) → {(_softSpeedOk ? "PASS" : "FAIL")}");
|
|
GD.Print($"WELD: soft settle — rot dev {Mathf.RadToDeg(settleRot):F2} deg (limit {SoftSettleRotLimitDeg}), sep {settleSep:F4} m (limit {SoftSettleSepLimit}) → {(_softSettleOk ? "PASS" : "FAIL")}");
|
|
bool overall = _typeOk && _rigidRotOk && _rigidSepOk && _rigidSpeedOk && _softStretchOk && _softSpeedOk && _softSettleOk;
|
|
GD.Print($"WELD: overall {(overall ? "PASS — solver-owned rigid weld via ConeTwistJoint3D functional on patched Box3D" : "CHECK — behavior differs, inspect above")}");
|
|
GetTree().Quit();
|
|
}
|
|
}
|