Initial commit: VR Messs workspace (grab v5.3.1, body-physics research)
This commit is contained in:
Foxyz1302 committed
commit
5d2c5bf265
57 files changed
+4990
No files matched your search
@@ -0,0 +1,227 @@
|
||||
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();
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user