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,487 @@
|
||||
using Godot;
|
||||
|
||||
public partial class GrabSpike : Node3D
|
||||
{
|
||||
private const float HandMass = 0.4f;
|
||||
private const float HandMaxForce = 90f;
|
||||
private const float HandMaxSpeed = 2.5f;
|
||||
private const float ReachSpeed = 1f;
|
||||
private const float HandMaxSpin = 120f;
|
||||
private const float HandRadius = 0.05f;
|
||||
private const float MoveDistance = 2f;
|
||||
private const float MoveDuration = 2f;
|
||||
private const float DragRampTime = 1f;
|
||||
private const float LiftHeight = 0.3f;
|
||||
private const float MaxSpeed = 20f;
|
||||
private const float ReachTimeout = 3f;
|
||||
private const float RigidWeldBias = 0f;
|
||||
private const float RigidRotLimitDeg = 2f;
|
||||
private const float RigidSepLimit = 0.01f;
|
||||
private const float SpinRotLimitDeg = 3.5f;
|
||||
private const float TakeoverRotLimitDeg = 5f;
|
||||
private const float SpinLimit = 40f;
|
||||
private const float PullDistance = 0.3f;
|
||||
private const float SpinAngleDeg = 360f;
|
||||
private const float SpinDuration = 2.5f;
|
||||
private const float SpinHoldDuration = 0.5f;
|
||||
private const float TugDuration = 1.5f;
|
||||
private const float SettleDuration = 0.5f;
|
||||
private const float FollowDuration = 1f;
|
||||
private const float TugPull = 0.5f;
|
||||
private const float FollowSway = 0.03f;
|
||||
private const float TwoHandDuration = TugDuration + FollowDuration;
|
||||
private const float TakeoverDuration = 1.5f;
|
||||
|
||||
private enum Phase { Reach, Drag, Spin, Hand2Reach, TwoHanded, TwoSettle, TwoFollow, Takeover }
|
||||
|
||||
private RigidBody3D _prop;
|
||||
private RigidBody3D _hand;
|
||||
private RigidBody3D _hand2;
|
||||
private Node3D _target;
|
||||
private Node3D _target2;
|
||||
private ConeTwistJoint3D _joint;
|
||||
private ConeTwistJoint3D _joint2;
|
||||
private Vector3 _grabPoint;
|
||||
private Vector3 _grab2Point;
|
||||
private Vector3 _propAnchor;
|
||||
private Vector3 _handAnchor;
|
||||
private Vector3 _propAnchor2;
|
||||
private Vector3 _handAnchor2;
|
||||
private Quaternion _relRotation1;
|
||||
private Quaternion _relRotation2;
|
||||
private Quaternion _spinFrom;
|
||||
private Quaternion _spinTo;
|
||||
private Quaternion _cmd1 = Quaternion.Identity;
|
||||
private Vector3 _takeoverBase;
|
||||
private Phase _phase = Phase.Reach;
|
||||
private float _phaseStart;
|
||||
private float _elapsed;
|
||||
private bool _dragRotOk;
|
||||
private bool _dragSepOk;
|
||||
private bool _dragSpeedOk;
|
||||
private bool _spinRotOk;
|
||||
private bool _spinSepOk;
|
||||
private bool _spinSpeedOk;
|
||||
private bool _twoHandedOk;
|
||||
private bool _takeoverRotOk;
|
||||
private bool _takeoverSepOk;
|
||||
private bool _takeoverSpeedOk;
|
||||
private float _maxDragRot;
|
||||
private float _maxDragSep;
|
||||
private float _maxDragPropSpeed;
|
||||
private float _maxDragHandSpeed;
|
||||
private float _maxSpinRot;
|
||||
private float _maxSpinSep;
|
||||
private float _maxSpinPropSpeed;
|
||||
private float _maxSpinHandSpeed;
|
||||
private float _maxTwoAnchor1;
|
||||
private float _maxTugAnchor2;
|
||||
private float _maxFollowAnchor2;
|
||||
private float _maxTwoPropSpeed;
|
||||
private float _maxTwoHandSpeed;
|
||||
private float _maxTwoHand2Speed;
|
||||
private float _maxTwoSpin;
|
||||
private float _maxTakeoverRot;
|
||||
private float _maxTakeoverSep;
|
||||
private float _maxTakeoverPropSpeed;
|
||||
private float _maxTakeoverHandSpeed;
|
||||
private int _lastLogStep;
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
StaticBody3D floor = new() { Position = new Vector3(0, -0.5f, 0) };
|
||||
floor.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(20, 1, 20) } });
|
||||
AddChild(floor);
|
||||
|
||||
_prop = new RigidBody3D { Position = new Vector3(0, 0.15f, 0), Rotation = new Vector3(0, Mathf.Pi / 2f, 0), Mass = 1f };
|
||||
_prop.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = Vector3.One * 0.3f } });
|
||||
AddChild(_prop);
|
||||
|
||||
_hand = new RigidBody3D
|
||||
{
|
||||
Position = new Vector3(0, 0.9f, 0),
|
||||
Mass = HandMass,
|
||||
GravityScale = 0f,
|
||||
ContinuousCd = true,
|
||||
ContactMonitor = true,
|
||||
MaxContactsReported = 4,
|
||||
};
|
||||
_hand.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = HandRadius } });
|
||||
_hand.BodyEntered += OnHand1Touched;
|
||||
AddChild(_hand);
|
||||
|
||||
_hand2 = new RigidBody3D
|
||||
{
|
||||
Position = new Vector3(0, 0.15f, -1f),
|
||||
Mass = HandMass,
|
||||
GravityScale = 0f,
|
||||
ContinuousCd = true,
|
||||
ContactMonitor = true,
|
||||
MaxContactsReported = 4,
|
||||
};
|
||||
_hand2.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = HandRadius } });
|
||||
_hand2.BodyEntered += OnHand2Touched;
|
||||
AddChild(_hand2);
|
||||
|
||||
_target = new Node3D { Position = _prop.Position };
|
||||
AddChild(_target);
|
||||
_target2 = new Node3D { Position = _hand2.Position };
|
||||
AddChild(_target2);
|
||||
GD.Print("GRAB: grab spike scene ready");
|
||||
}
|
||||
|
||||
private void OnHand1Touched(Node body)
|
||||
{
|
||||
if (body != _prop || _phase != Phase.Reach)
|
||||
return;
|
||||
_grabPoint = _hand.GlobalPosition;
|
||||
_relRotation1 = _hand.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
||||
* _prop.GlobalTransform.Basis.GetRotationQuaternion();
|
||||
_propAnchor = _prop.GlobalTransform.AffineInverse() * _grabPoint;
|
||||
_handAnchor = _hand.GlobalTransform.AffineInverse() * _grabPoint;
|
||||
_joint = MakeWeld(_hand, _prop, _grabPoint, RigidWeldBias);
|
||||
_maxDragRot = 0f;
|
||||
_maxDragSep = 0f;
|
||||
_maxDragPropSpeed = 0f;
|
||||
_maxDragHandSpeed = 0f;
|
||||
_phase = Phase.Drag;
|
||||
_phaseStart = _elapsed;
|
||||
_lastLogStep = 0;
|
||||
GD.Print($"GRAB: rigid weld grab at t={_elapsed:F2} s, pivot {_grabPoint}, server joint type = {PhysicsServer3D.JointGetType(_joint.GetRid())} (expect {PhysicsServer3D.JointType.ConeTwist})");
|
||||
}
|
||||
|
||||
private void OnHand2Touched(Node body)
|
||||
{
|
||||
if (body != _prop || _phase != Phase.Hand2Reach)
|
||||
return;
|
||||
_grab2Point = _hand2.GlobalPosition;
|
||||
_relRotation2 = _hand2.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
||||
* _prop.GlobalTransform.Basis.GetRotationQuaternion();
|
||||
_propAnchor2 = _prop.GlobalTransform.AffineInverse() * _grab2Point;
|
||||
_handAnchor2 = _hand2.GlobalTransform.AffineInverse() * _grab2Point;
|
||||
_joint2 = MakeWeld(_hand2, _prop, _grab2Point, RigidWeldBias);
|
||||
_maxTwoAnchor1 = 0f;
|
||||
_maxTugAnchor2 = 0f;
|
||||
_maxFollowAnchor2 = 0f;
|
||||
_maxTwoPropSpeed = 0f;
|
||||
_maxTwoHandSpeed = 0f;
|
||||
_maxTwoHand2Speed = 0f;
|
||||
_maxTwoSpin = 0f;
|
||||
_phase = Phase.TwoHanded;
|
||||
_phaseStart = _elapsed;
|
||||
_lastLogStep = 0;
|
||||
GD.Print($"GRAB: second hand rigid weld at t={_elapsed:F2} s, pivot {_grab2Point}");
|
||||
}
|
||||
|
||||
public override void _PhysicsProcess(double delta)
|
||||
{
|
||||
float dt = (float)delta;
|
||||
_elapsed += dt;
|
||||
float phaseT = _elapsed - _phaseStart;
|
||||
|
||||
switch (_phase)
|
||||
{
|
||||
case Phase.Reach:
|
||||
_target.GlobalPosition = _prop.GlobalPosition;
|
||||
_target2.GlobalPosition = _hand2.GlobalPosition;
|
||||
if (phaseT > ReachTimeout)
|
||||
Fail("hand never touched the prop");
|
||||
break;
|
||||
case Phase.Drag:
|
||||
{
|
||||
_target.GlobalPosition = _grabPoint + new Vector3(DragOffset(phaseT), LiftHeight * Mathf.Min(phaseT / DragRampTime, 1f), 0f);
|
||||
_maxDragPropSpeed = Mathf.Max(_maxDragPropSpeed, _prop.LinearVelocity.Length());
|
||||
_maxDragHandSpeed = Mathf.Max(_maxDragHandSpeed, _hand.LinearVelocity.Length());
|
||||
TrackRigidWeld(ref _maxDragRot, ref _maxDragSep);
|
||||
int step = (int)(phaseT / 0.5f);
|
||||
if (step > _lastLogStep && phaseT < MoveDuration)
|
||||
{
|
||||
_lastLogStep = step;
|
||||
GD.Print($"GRAB: drag t={phaseT:F2} rotDev={Mathf.RadToDeg(RotDev1()):F2} deg sep={Sep1():F4} m propV={_prop.LinearVelocity.Length():F2}");
|
||||
}
|
||||
if (phaseT >= MoveDuration)
|
||||
FinishDrag();
|
||||
break;
|
||||
}
|
||||
case Phase.Spin:
|
||||
{
|
||||
_cmd1 = _spinFrom * new Quaternion(Vector3.Up,
|
||||
Mathf.DegToRad(SpinAngleDeg) * Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / SpinDuration, 1f)));
|
||||
_target.GlobalPosition = _grabPoint + new Vector3(MoveDistance, LiftHeight, 0f);
|
||||
_maxSpinPropSpeed = Mathf.Max(_maxSpinPropSpeed, _prop.LinearVelocity.Length());
|
||||
_maxSpinHandSpeed = Mathf.Max(_maxSpinHandSpeed, _hand.LinearVelocity.Length());
|
||||
TrackRigidWeld(ref _maxSpinRot, ref _maxSpinSep);
|
||||
int step = (int)(phaseT / 0.5f);
|
||||
if (step > _lastLogStep)
|
||||
{
|
||||
_lastLogStep = step;
|
||||
GD.Print($"GRAB: fast spin t={phaseT:F2} rotDev={Mathf.RadToDeg(RotDev1()):F2} deg sep={Sep1():F4} m handW={_hand.AngularVelocity.Length():F1} rad/s");
|
||||
}
|
||||
if (phaseT >= SpinDuration + SpinHoldDuration)
|
||||
FinishSpin();
|
||||
break;
|
||||
}
|
||||
case Phase.Hand2Reach:
|
||||
_target.GlobalPosition = _grabPoint + new Vector3(MoveDistance, LiftHeight, 0f);
|
||||
_target2.GlobalPosition = _prop.GlobalPosition;
|
||||
if (phaseT > ReachTimeout)
|
||||
Fail("second hand never touched the prop");
|
||||
break;
|
||||
case Phase.TwoHanded:
|
||||
{
|
||||
float pull = TugPull * Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / 1f, 1f));
|
||||
float zSway = 0.35f * Mathf.Sin(2f * phaseT) * (1f - Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / TugDuration, 1f)));
|
||||
_target.GlobalPosition = _grabPoint + new Vector3(MoveDistance + pull, LiftHeight, zSway);
|
||||
_target2.GlobalPosition = _grab2Point + new Vector3(-pull, 0f, -zSway);
|
||||
_maxTwoAnchor1 = Mathf.Max(_maxTwoAnchor1, Sep1());
|
||||
_maxTugAnchor2 = Mathf.Max(_maxTugAnchor2, Sep2());
|
||||
_maxTwoPropSpeed = Mathf.Max(_maxTwoPropSpeed, _prop.LinearVelocity.Length());
|
||||
_maxTwoHandSpeed = Mathf.Max(_maxTwoHandSpeed, _hand.LinearVelocity.Length());
|
||||
_maxTwoHand2Speed = Mathf.Max(_maxTwoHand2Speed, _hand2.LinearVelocity.Length());
|
||||
_maxTwoSpin = Mathf.Max(_maxTwoSpin, _prop.AngularVelocity.Length());
|
||||
int step = (int)(phaseT / 0.5f);
|
||||
if (step > _lastLogStep)
|
||||
{
|
||||
_lastLogStep = step;
|
||||
GD.Print($"GRAB: two-handed tug t={phaseT:F2} anchor1={Sep1():F3} anchor2={Sep2():F3} spin={_prop.AngularVelocity.Length():F1} propV={_prop.LinearVelocity.Length():F2}");
|
||||
}
|
||||
if (phaseT >= TugDuration)
|
||||
{
|
||||
_phase = Phase.TwoSettle;
|
||||
_phaseStart = _elapsed;
|
||||
_lastLogStep = 0;
|
||||
GD.Print("GRAB: tug done, settling before the follow assert");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Phase.TwoSettle:
|
||||
{
|
||||
_target.GlobalPosition = _grabPoint + new Vector3(MoveDistance + TugPull, LiftHeight, 0f);
|
||||
_target2.GlobalPosition = _prop.GlobalTransform * _propAnchor2;
|
||||
if (phaseT >= SettleDuration)
|
||||
{
|
||||
_phase = Phase.TwoFollow;
|
||||
_phaseStart = _elapsed;
|
||||
_lastLogStep = 0;
|
||||
GD.Print($"GRAB: settled at anchor2={Sep2():F3} m, second hand now follows with a small out-of-phase sway");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Phase.TwoFollow:
|
||||
{
|
||||
_target.GlobalPosition = _grabPoint + new Vector3(MoveDistance + TugPull, LiftHeight, 0f);
|
||||
_target2.GlobalPosition = _prop.GlobalTransform * _propAnchor2 + new Vector3(0f, 0f, -FollowSway * Mathf.Sin(2f * Mathf.Pi * 1.5f * phaseT));
|
||||
_maxTwoAnchor1 = Mathf.Max(_maxTwoAnchor1, Sep1());
|
||||
_maxFollowAnchor2 = Mathf.Max(_maxFollowAnchor2, Sep2());
|
||||
_maxTwoPropSpeed = Mathf.Max(_maxTwoPropSpeed, _prop.LinearVelocity.Length());
|
||||
_maxTwoHandSpeed = Mathf.Max(_maxTwoHandSpeed, _hand.LinearVelocity.Length());
|
||||
_maxTwoHand2Speed = Mathf.Max(_maxTwoHand2Speed, _hand2.LinearVelocity.Length());
|
||||
_maxTwoSpin = Mathf.Max(_maxTwoSpin, _prop.AngularVelocity.Length());
|
||||
int step = (int)(phaseT / 0.5f);
|
||||
if (step > _lastLogStep)
|
||||
{
|
||||
_lastLogStep = step;
|
||||
GD.Print($"GRAB: two-handed follow t={phaseT:F2} anchor1={Sep1():F3} anchor2={Sep2():F3} propV={_prop.LinearVelocity.Length():F2}");
|
||||
}
|
||||
if (phaseT >= FollowDuration)
|
||||
FinishTwoHanded();
|
||||
break;
|
||||
}
|
||||
case Phase.Takeover:
|
||||
{
|
||||
_target.GlobalPosition = _grabPoint + new Vector3(MoveDistance, LiftHeight, 0f);
|
||||
float move = PullDistance * Mathf.SmoothStep(0f, 1f, Mathf.Min(phaseT / TakeoverDuration, 1f));
|
||||
_target2.GlobalPosition = _takeoverBase + new Vector3(-move, 0f, 0f);
|
||||
_maxTakeoverPropSpeed = Mathf.Max(_maxTakeoverPropSpeed, _prop.LinearVelocity.Length());
|
||||
_maxTakeoverHandSpeed = Mathf.Max(_maxTakeoverHandSpeed, _hand2.LinearVelocity.Length());
|
||||
TrackTakeoverWeld();
|
||||
int step = (int)(phaseT / 0.5f);
|
||||
if (step > _lastLogStep)
|
||||
{
|
||||
_lastLogStep = step;
|
||||
GD.Print($"GRAB: takeover t={phaseT:F2} rotDev={Mathf.RadToDeg(RotDev2()):F2} deg sep={Sep2():F4} m propV={_prop.LinearVelocity.Length():F2}");
|
||||
}
|
||||
if (phaseT >= TakeoverDuration)
|
||||
Finish();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Chase(_hand, _target, dt, _phase == Phase.Reach || _phase == Phase.Hand2Reach ? ReachSpeed : HandMaxSpeed);
|
||||
Chase(_hand2, _target2, dt, _phase == Phase.Reach || _phase == Phase.Hand2Reach ? ReachSpeed : HandMaxSpeed);
|
||||
SteerHand(_hand, _cmd1, dt);
|
||||
SteerHand(_hand2, Quaternion.Identity, dt);
|
||||
}
|
||||
|
||||
private ConeTwistJoint3D MakeWeld(RigidBody3D hand, RigidBody3D prop, Vector3 pivot, float bias)
|
||||
{
|
||||
ConeTwistJoint3D weld = new()
|
||||
{
|
||||
ExcludeNodesFromCollision = true,
|
||||
Position = pivot,
|
||||
NodeA = hand.GetPath(),
|
||||
NodeB = prop.GetPath(),
|
||||
SwingSpan = 0f,
|
||||
TwistSpan = 0f,
|
||||
Bias = bias,
|
||||
};
|
||||
AddChild(weld);
|
||||
return weld;
|
||||
}
|
||||
|
||||
private void TrackRigidWeld(ref float maxRot, ref float maxSep)
|
||||
{
|
||||
maxRot = Mathf.Max(maxRot, RotDev1());
|
||||
maxSep = Mathf.Max(maxSep, Sep1());
|
||||
}
|
||||
|
||||
private void TrackTakeoverWeld()
|
||||
{
|
||||
Quaternion relNow = _hand2.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
||||
* _prop.GlobalTransform.Basis.GetRotationQuaternion();
|
||||
_maxTakeoverRot = Mathf.Max(_maxTakeoverRot, DevAngle(relNow, _relRotation2));
|
||||
_maxTakeoverSep = Mathf.Max(_maxTakeoverSep, Sep2());
|
||||
}
|
||||
|
||||
private float RotDev1() => DevAngle(
|
||||
_hand.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
||||
* _prop.GlobalTransform.Basis.GetRotationQuaternion(),
|
||||
_relRotation1);
|
||||
|
||||
private float RotDev2() => DevAngle(
|
||||
_hand2.GlobalTransform.Basis.GetRotationQuaternion().Inverse()
|
||||
* _prop.GlobalTransform.Basis.GetRotationQuaternion(),
|
||||
_relRotation2);
|
||||
|
||||
private float Sep1() =>
|
||||
(_prop.GlobalTransform * _propAnchor).DistanceTo(_hand.GlobalTransform * _handAnchor);
|
||||
|
||||
private float Sep2() =>
|
||||
(_prop.GlobalTransform * _propAnchor2).DistanceTo(_hand2.GlobalTransform * _handAnchor2);
|
||||
|
||||
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 WeldSpike does
|
||||
if (float.IsNaN(dev.W))
|
||||
return float.NaN;
|
||||
return 2f * Mathf.Acos(Mathf.Clamp(Mathf.Abs(dev.W), 0f, 1f));
|
||||
}
|
||||
|
||||
private static float DragOffset(float t)
|
||||
{
|
||||
float cruiseSpeed = MoveDistance / (MoveDuration - DragRampTime);
|
||||
if (t <= DragRampTime)
|
||||
return 0.5f * cruiseSpeed / DragRampTime * t * t;
|
||||
if (t <= MoveDuration - DragRampTime)
|
||||
return 0.5f * cruiseSpeed * DragRampTime + cruiseSpeed * (t - DragRampTime);
|
||||
float remaining = MoveDuration - t;
|
||||
return MoveDistance - 0.5f * cruiseSpeed / DragRampTime * remaining * remaining;
|
||||
}
|
||||
|
||||
private static void SteerHand(RigidBody3D hand, Quaternion cmd, float dt)
|
||||
{
|
||||
Quaternion q = cmd * hand.GlobalTransform.Basis.GetRotationQuaternion().Inverse();
|
||||
Vector3 spin = q.GetAxis() * (q.GetAngle() / dt);
|
||||
if (spin.Length() > HandMaxSpin)
|
||||
spin = spin.Normalized() * HandMaxSpin;
|
||||
hand.AngularVelocity = spin;
|
||||
}
|
||||
|
||||
private static void Chase(RigidBody3D body, Node3D target, float dt, float maxSpeed)
|
||||
{
|
||||
Vector3 to = target.GlobalPosition - body.GlobalPosition;
|
||||
Vector3 desired = to / dt;
|
||||
if (desired.Length() > maxSpeed)
|
||||
desired = desired.Normalized() * maxSpeed;
|
||||
Vector3 dv = desired - body.LinearVelocity;
|
||||
Vector3 force = dv / dt * HandMass;
|
||||
if (force.Length() > HandMaxForce)
|
||||
force = force.Normalized() * HandMaxForce;
|
||||
body.LinearVelocity += force / HandMass * dt;
|
||||
}
|
||||
|
||||
private void FinishDrag()
|
||||
{
|
||||
_dragRotOk = _maxDragRot < Mathf.DegToRad(RigidRotLimitDeg) && !float.IsNaN(_maxDragRot);
|
||||
_dragSepOk = _maxDragSep < RigidSepLimit;
|
||||
_dragSpeedOk = _maxDragPropSpeed < MaxSpeed && _maxDragHandSpeed < MaxSpeed;
|
||||
GD.Print($"GRAB: drag rigid — rot dev max {Mathf.RadToDeg(_maxDragRot):F2} deg (limit {RigidRotLimitDeg}), sep max {_maxDragSep:F4} m (limit {RigidSepLimit}) → {(_dragRotOk && _dragSepOk ? "PASS" : "FAIL")}");
|
||||
GD.Print($"GRAB: drag speeds — prop max {_maxDragPropSpeed:F2} m/s, hand max {_maxDragHandSpeed:F2} m/s (limit {MaxSpeed}) → {(_dragSpeedOk ? "PASS" : "FAIL")}");
|
||||
_spinFrom = _hand.GlobalTransform.Basis.GetRotationQuaternion();
|
||||
_spinTo = _spinFrom * new Quaternion(Vector3.Up, Mathf.DegToRad(SpinAngleDeg));
|
||||
_maxSpinRot = 0f;
|
||||
_maxSpinSep = 0f;
|
||||
_maxSpinPropSpeed = 0f;
|
||||
_maxSpinHandSpeed = 0f;
|
||||
_phase = Phase.Spin;
|
||||
_phaseStart = _elapsed;
|
||||
_lastLogStep = 0;
|
||||
GD.Print($"GRAB: fast spin starting — {SpinAngleDeg:F0} deg in {SpinDuration:0.#} s around world up"); }
|
||||
|
||||
private void FinishSpin()
|
||||
{
|
||||
_spinRotOk = _maxSpinRot < Mathf.DegToRad(SpinRotLimitDeg) && !float.IsNaN(_maxSpinRot);
|
||||
_spinSepOk = _maxSpinSep < RigidSepLimit;
|
||||
_spinSpeedOk = _maxSpinPropSpeed < MaxSpeed && _maxSpinHandSpeed < MaxSpeed;
|
||||
GD.Print($"GRAB: fast spin rigid — rot dev max {Mathf.RadToDeg(_maxSpinRot):F2} deg (limit {SpinRotLimitDeg}), sep max {_maxSpinSep:F4} m (limit {RigidSepLimit}) → {(_spinRotOk && _spinSepOk ? "PASS" : "FAIL")}");
|
||||
GD.Print($"GRAB: fast spin speeds — prop max {_maxSpinPropSpeed:F2} m/s, hand max {_maxSpinHandSpeed:F2} m/s (limit {MaxSpeed}) → {(_spinSpeedOk ? "PASS" : "FAIL")}");
|
||||
_hand.AngularVelocity = Vector3.Zero;
|
||||
_hand2.GlobalTransform = new Transform3D(Basis.Identity, _prop.GlobalPosition + new Vector3(0.45f, 0f, 0f));
|
||||
_hand2.LinearVelocity = Vector3.Zero;
|
||||
_hand2.AngularVelocity = Vector3.Zero;
|
||||
_phase = Phase.Hand2Reach;
|
||||
_phaseStart = _elapsed;
|
||||
_lastLogStep = 0;
|
||||
}
|
||||
|
||||
private void FinishTwoHanded()
|
||||
{
|
||||
bool primaryRigidOk = _maxTwoAnchor1 < RigidSepLimit;
|
||||
bool tugRigidOk = _maxTugAnchor2 < RigidSepLimit;
|
||||
bool followOk = _maxFollowAnchor2 < RigidSepLimit;
|
||||
bool spinOk = _maxTwoSpin < SpinLimit;
|
||||
bool speedOk = _maxTwoPropSpeed < MaxSpeed && _maxTwoHandSpeed < MaxSpeed && _maxTwoHand2Speed < MaxSpeed;
|
||||
GD.Print($"GRAB: two-handed primary rigid — anchor1 max {_maxTwoAnchor1:F4} m (limit {RigidSepLimit}) → {(primaryRigidOk ? "PASS" : "FAIL")}");
|
||||
GD.Print($"GRAB: two-handed tug rigid — anchor2 max {_maxTugAnchor2:F4} m (limit {RigidSepLimit}) → {(tugRigidOk ? "PASS" : "FAIL")}");
|
||||
GD.Print($"GRAB: two-handed follow — anchor2 max {_maxFollowAnchor2:F4} m (limit {RigidSepLimit}) → {(followOk ? "PASS" : "FAIL")}");
|
||||
GD.Print($"GRAB: two-handed spin — prop max {_maxTwoSpin:F1} rad/s (limit {SpinLimit}) → {(spinOk ? "PASS" : "FAIL")}");
|
||||
GD.Print($"GRAB: two-handed speed — prop max {_maxTwoPropSpeed:F2} m/s, hands max {Mathf.Max(_maxTwoHandSpeed, _maxTwoHand2Speed):F2} m/s (limit {MaxSpeed}) → {(speedOk ? "PASS" : "FAIL")}");
|
||||
_twoHandedOk = primaryRigidOk && tugRigidOk && followOk && spinOk && speedOk;
|
||||
|
||||
_joint.QueueFree();
|
||||
_joint = null;
|
||||
_takeoverBase = _target2.GlobalPosition;
|
||||
_maxTakeoverRot = 0f;
|
||||
_maxTakeoverSep = 0f;
|
||||
_maxTakeoverPropSpeed = 0f;
|
||||
_maxTakeoverHandSpeed = 0f;
|
||||
_phase = Phase.Takeover;
|
||||
_phaseStart = _elapsed;
|
||||
_lastLogStep = 0;
|
||||
GD.Print($"GRAB: primary released, surviving rigid weld untouched (pivot {_grab2Point})");
|
||||
}
|
||||
|
||||
private void Finish()
|
||||
{
|
||||
_takeoverRotOk = _maxTakeoverRot < Mathf.DegToRad(TakeoverRotLimitDeg) && !float.IsNaN(_maxTakeoverRot);
|
||||
_takeoverSepOk = _maxTakeoverSep < RigidSepLimit;
|
||||
_takeoverSpeedOk = _maxTakeoverPropSpeed < MaxSpeed && _maxTakeoverHandSpeed < MaxSpeed;
|
||||
GD.Print($"GRAB: takeover rigid — rot dev max {Mathf.RadToDeg(_maxTakeoverRot):F2} deg (limit {TakeoverRotLimitDeg}), sep max {_maxTakeoverSep:F4} m (limit {RigidSepLimit}) → {(_takeoverRotOk && _takeoverSepOk ? "PASS" : "FAIL")}");
|
||||
GD.Print($"GRAB: takeover speeds — prop max {_maxTakeoverPropSpeed:F2} m/s, hand2 max {_maxTakeoverHandSpeed:F2} m/s (limit {MaxSpeed}) → {(_takeoverSpeedOk ? "PASS" : "FAIL")}");
|
||||
bool overall = _dragRotOk && _dragSepOk && _dragSpeedOk &&
|
||||
_spinRotOk && _spinSepOk && _spinSpeedOk &&
|
||||
_twoHandedOk &&
|
||||
_takeoverRotOk && _takeoverSepOk && _takeoverSpeedOk;
|
||||
GD.Print($"GRAB: overall {(overall ? "PASS — solver-owned rigid weld grab (v5.3) functional on Box3D" : "CHECK — behavior differs, inspect above")}");
|
||||
GetTree().Quit();
|
||||
}
|
||||
|
||||
private void Fail(string reason)
|
||||
{
|
||||
GD.Print($"GRAB: overall CHECK — {reason}");
|
||||
GetTree().Quit();
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user