329 lines
13 KiB
C#
329 lines
13 KiB
C#
using System.Collections.Generic;
|
|
using Godot;
|
|
|
|
public partial class BodySpike : Node3D
|
|
{
|
|
private enum Phase { Settle, Pairs, Track, Yaw, Impact, Snap, Upright }
|
|
|
|
private const float PhaseSettle = 0.8f;
|
|
private const float PhasePairs1 = 1.3f;
|
|
private const float PhasePairs2 = 1.7f;
|
|
private const float PhaseTrack = 3.5f;
|
|
private const float PhaseYaw = 2f;
|
|
private const float PhaseImpact = 2f;
|
|
private const float PhaseSnap = 0.3f;
|
|
private const float PhaseUpright = 2f;
|
|
|
|
private const float TrackPosLimit = 0.06f;
|
|
private const float TrackSpeedLimit = 8f;
|
|
private const float YawPeakLimitDeg = 15f;
|
|
private const float YawSettleLimitDeg = 4f;
|
|
private const float ImpactDipLimit = 0.2f;
|
|
private const float RecoverLimit = 0.02f;
|
|
private const float ImpactorMinDistLimit = 0.1f;
|
|
private const float ImpactorSpeedAfterHitLimit = 1.5f;
|
|
private const float SnapLimit = 0.05f;
|
|
private const float UprightLimitDeg = 3f;
|
|
private const float YawHoldLimitDeg = 5f;
|
|
|
|
private PhysicsTorso _torso;
|
|
private Node3D _head;
|
|
private RigidBody3D _ball;
|
|
private RigidBody3D _handProbe;
|
|
private RigidBody3D _prop;
|
|
private RigidBody3D _impactor;
|
|
|
|
private Phase _phase = Phase.Settle;
|
|
private float _phaseStart;
|
|
private float _elapsed;
|
|
private readonly List<string> _resultNames = new();
|
|
private readonly List<bool> _resultOk = new();
|
|
|
|
private Vector3 _chestRef;
|
|
private bool _chestStillDuringHandDrop;
|
|
private float _chestDropDip;
|
|
private float _maxTrackErr;
|
|
private float _maxTrackSpeed;
|
|
private float _maxYawErrAfterRamp;
|
|
private float _yawErrEnd;
|
|
private float _maxImpactDip;
|
|
private float _minImpactorDist = float.MaxValue;
|
|
private float _impactorTouchTime = -1f;
|
|
private float _impactorSpeedAfterHit = -1f;
|
|
|
|
public override void _Ready()
|
|
{
|
|
BuildWorld();
|
|
_head = new Node3D { Position = new Vector3(0f, 1.6f, 0f) };
|
|
AddChild(_head);
|
|
|
|
_torso = new PhysicsTorso { HeadSource = _head };
|
|
AddChild(_torso);
|
|
GD.Print("BODY: body spike scene ready");
|
|
}
|
|
|
|
private void BuildWorld()
|
|
{
|
|
StaticBody3D ground = new() { CollisionLayer = 1, CollisionMask = 0 };
|
|
ground.AddChild(new CollisionShape3D
|
|
{
|
|
Shape = new BoxShape3D { Size = new Vector3(20f, 0.1f, 20f) },
|
|
Position = new Vector3(0f, -0.05f, 0f),
|
|
});
|
|
AddChild(ground);
|
|
|
|
_ball = new RigidBody3D
|
|
{
|
|
Mass = 1f,
|
|
Position = new Vector3(-2f, 0.4f, 0f),
|
|
CollisionLayer = 1,
|
|
CollisionMask = 1,
|
|
PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.8f },
|
|
};
|
|
_ball.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.05f } });
|
|
AddChild(_ball);
|
|
|
|
_handProbe = new RigidBody3D
|
|
{
|
|
Mass = 0.4f,
|
|
Position = new Vector3(0f, 1.7f, 0.12f),
|
|
CollisionLayer = 0b1000,
|
|
CollisionMask = 0b1001,
|
|
PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0f, Friction = 0.8f },
|
|
};
|
|
_handProbe.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(0.035f, 0.09f, 0.12f) } });
|
|
AddChild(_handProbe);
|
|
|
|
_prop = new RigidBody3D
|
|
{
|
|
Mass = 2f,
|
|
Position = new Vector3(0f, 1.9f, 0.12f),
|
|
CollisionLayer = 1,
|
|
CollisionMask = 0b1101,
|
|
PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.3f, Friction = 0.8f },
|
|
};
|
|
_prop.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } });
|
|
AddChild(_prop);
|
|
_prop.Freeze = true;
|
|
_prop.Visible = false;
|
|
|
|
_impactor = new RigidBody3D
|
|
{
|
|
Mass = 5f,
|
|
GravityScale = 0f,
|
|
CollisionLayer = 1,
|
|
CollisionMask = 0b1101,
|
|
PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0.2f, Friction = 0.6f },
|
|
};
|
|
_impactor.AddChild(new CollisionShape3D { Shape = new SphereShape3D { Radius = 0.06f } });
|
|
AddChild(_impactor);
|
|
_impactor.Freeze = true;
|
|
}
|
|
|
|
public override void _PhysicsProcess(double delta)
|
|
{
|
|
float dt = (float)delta;
|
|
_elapsed += dt;
|
|
float phaseT = _elapsed - _phaseStart;
|
|
|
|
Vector3 p = _torso.Body.GlobalPosition;
|
|
if (float.IsNaN(p.X) || float.IsNaN(p.Y) || float.IsNaN(p.Z))
|
|
{
|
|
Record("no NaN in body state", false);
|
|
Finish();
|
|
return;
|
|
}
|
|
|
|
switch (_phase)
|
|
{
|
|
case Phase.Settle:
|
|
if (phaseT >= PhaseSettle)
|
|
{
|
|
_chestRef = _torso.Body.GlobalPosition;
|
|
StartPhase(Phase.Pairs);
|
|
GD.Print("BODY: pairs phase — one-way floor, hand-through-chest exclusion, prop contact");
|
|
}
|
|
break;
|
|
|
|
case Phase.Pairs:
|
|
{
|
|
if (phaseT >= PhasePairs1 && _prop.Freeze)
|
|
{
|
|
_chestStillDuringHandDrop = _torso.Body.GlobalPosition.DistanceTo(_chestRef) < 0.02f;
|
|
_chestRef = _torso.Body.GlobalPosition;
|
|
_chestDropDip = 0f;
|
|
_prop.Freeze = false;
|
|
_prop.Visible = true;
|
|
GD.Print("BODY: dropping 2 kg ball onto chest top");
|
|
}
|
|
_chestDropDip = Mathf.Max(_chestDropDip, _torso.Body.GlobalPosition.DistanceTo(_chestRef));
|
|
if (phaseT >= PhasePairs1 + PhasePairs2)
|
|
FinishPairs();
|
|
break;
|
|
}
|
|
|
|
case Phase.Track:
|
|
{
|
|
float t = phaseT;
|
|
Vector3 headPos = new(0.6f * Mathf.Sin(1.1f * t), 1.6f, 0.8f * Mathf.Sin(0.7f * t));
|
|
Basis headBasis = new(new Quaternion(Vector3.Up, 0.6f * Mathf.Sin(0.5f * t)));
|
|
_head.GlobalTransform = new Transform3D(headBasis, headPos);
|
|
if (phaseT > 0.5f)
|
|
{
|
|
_maxTrackErr = Mathf.Max(_maxTrackErr, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
|
|
_maxTrackSpeed = Mathf.Max(_maxTrackSpeed, _torso.Body.LinearVelocity.Length());
|
|
}
|
|
if (phaseT >= PhaseTrack)
|
|
{
|
|
Record($"track error max {_maxTrackErr:F3} m (limit {TrackPosLimit})", _maxTrackErr < TrackPosLimit);
|
|
Record($"track speed max {_maxTrackSpeed:F2} m/s (limit {TrackSpeedLimit})", _maxTrackSpeed < TrackSpeedLimit);
|
|
StartPhase(Phase.Yaw);
|
|
GD.Print("BODY: yaw phase — head turns 90 deg over 1 s");
|
|
}
|
|
break;
|
|
}
|
|
|
|
case Phase.Yaw:
|
|
{
|
|
float yaw = Mathf.Pi / 2f * Mathf.Min(phaseT / 1f, 1f);
|
|
_head.GlobalTransform = new Transform3D(new Basis(new Quaternion(Vector3.Up, yaw)), _head.GlobalPosition);
|
|
float err = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis));
|
|
if (phaseT > 0.4f)
|
|
_maxYawErrAfterRamp = Mathf.Max(_maxYawErrAfterRamp, err);
|
|
if (phaseT > PhaseYaw - 0.5f)
|
|
_yawErrEnd = Mathf.Max(_yawErrEnd, err);
|
|
if (phaseT >= PhaseYaw)
|
|
{
|
|
Record($"yaw error peak {_maxYawErrAfterRamp:F1} deg (limit {YawPeakLimitDeg})", _maxYawErrAfterRamp < YawPeakLimitDeg);
|
|
Record($"yaw settled {_yawErrEnd:F1} deg (limit {YawSettleLimitDeg})", _yawErrEnd < YawSettleLimitDeg);
|
|
StartPhase(Phase.Impact);
|
|
GD.Print("BODY: impact phase — 5 kg ball at 3 m/s into the chest");
|
|
}
|
|
break;
|
|
}
|
|
|
|
case Phase.Impact:
|
|
{
|
|
_maxImpactDip = Mathf.Max(_maxImpactDip, _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition));
|
|
_minImpactorDist = Mathf.Min(_minImpactorDist, _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition));
|
|
if (_impactorTouchTime >= 0f && _impactorSpeedAfterHit < 0f && phaseT > _impactorTouchTime + 0.2f)
|
|
_impactorSpeedAfterHit = _impactor.LinearVelocity.Length();
|
|
if (phaseT >= PhaseImpact)
|
|
FinishImpact();
|
|
break;
|
|
}
|
|
|
|
case Phase.Snap:
|
|
{
|
|
if (phaseT >= PhaseSnap)
|
|
{
|
|
float err = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition);
|
|
Record($"snap re-locate (err {err:F3} m, limit {SnapLimit})", err < SnapLimit);
|
|
StartPhase(Phase.Upright);
|
|
_torso.Body.AngularVelocity += _torso.Body.GlobalTransform.Basis.X * 6f;
|
|
GD.Print("BODY: upright phase — pitch impulse, body must self-right");
|
|
}
|
|
break;
|
|
}
|
|
|
|
case Phase.Upright:
|
|
{
|
|
if (phaseT >= PhaseUpright)
|
|
FinishUpright();
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (_phase == Phase.Impact && _impactorTouchTime < 0f
|
|
&& _impactor.GlobalPosition.DistanceTo(_torso.Body.GlobalPosition) < 0.25f)
|
|
_impactorTouchTime = _elapsed - _phaseStart;
|
|
}
|
|
|
|
private void StartPhase(Phase phase)
|
|
{
|
|
_phase = phase;
|
|
_phaseStart = _elapsed;
|
|
if (phase == Phase.Impact)
|
|
{
|
|
_maxImpactDip = 0f;
|
|
_minImpactorDist = float.MaxValue;
|
|
_impactorTouchTime = -1f;
|
|
_impactorSpeedAfterHit = -1f;
|
|
_impactor.Freeze = false;
|
|
Vector3 chest = _torso.Body.GlobalPosition;
|
|
Vector3 fwd = -_torso.Body.GlobalTransform.Basis.Z;
|
|
_impactor.GlobalPosition = chest + fwd * 0.5f + Vector3.Up * 0.05f;
|
|
_impactor.LinearVelocity = -fwd * 3f;
|
|
_impactor.Visible = true;
|
|
GD.Print($"BODY: impactor armed freeze={_impactor.Freeze} pos={_impactor.GlobalPosition} v={_impactor.LinearVelocity}");
|
|
}
|
|
}
|
|
|
|
private void FinishPairs()
|
|
{
|
|
float chestY = _torso.Body.GlobalPosition.Y;
|
|
bool handBelow = _handProbe.GlobalPosition.Y < chestY - 0.4f;
|
|
Record($"hand-config body falls through own chest (hand y {_handProbe.GlobalPosition.Y:F2}, chest y {chestY:F2})", handBelow);
|
|
Record("chest undisturbed while hand-config body falls through", _chestStillDuringHandDrop);
|
|
bool propContact = _chestDropDip > 0.0001f;
|
|
bool propBounded = _chestDropDip < 0.1f;
|
|
Record($"dropped ball transfers contact into chest (dip {_chestDropDip:F4} m)", propContact && propBounded);
|
|
_prop.LinearVelocity = Vector3.Zero;
|
|
_prop.GlobalPosition = new Vector3(-2f, 0.3f, 1f);
|
|
StartPhase(Phase.Track);
|
|
GD.Print("BODY: track phase — head walks a smooth path");
|
|
}
|
|
|
|
private void FinishImpact()
|
|
{
|
|
Record($"impact chest dip max {_maxImpactDip:F3} m (limit {ImpactDipLimit})", _maxImpactDip < ImpactDipLimit);
|
|
float recovered = _torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition);
|
|
Record($"impact recovery {recovered:F3} m (limit {RecoverLimit})", recovered < RecoverLimit);
|
|
Record($"impactor never passed through (min dist {_minImpactorDist:F3} m, limit {ImpactorMinDistLimit})", _minImpactorDist > ImpactorMinDistLimit);
|
|
Record($"impactor slowed after hit (|v| {_impactorSpeedAfterHit:F2} m/s, limit {ImpactorSpeedAfterHitLimit})",
|
|
_impactorSpeedAfterHit >= 0f && _impactorSpeedAfterHit < ImpactorSpeedAfterHitLimit);
|
|
StartPhase(Phase.Snap);
|
|
_head.GlobalPosition += new Vector3(2.5f, 0f, 0f);
|
|
GD.Print("BODY: snap phase — head teleported 2.5 m");
|
|
}
|
|
|
|
private void FinishUpright()
|
|
{
|
|
float up = _torso.Body.GlobalTransform.Basis.Y.Dot(Vector3.Up);
|
|
float uprightDeg = Mathf.RadToDeg(Mathf.Acos(Mathf.Clamp(up, -1f, 1f)));
|
|
float yawErr = Mathf.Abs(YawErrorDeg(_torso.Body.GlobalTransform.Basis, _head.GlobalTransform.Basis));
|
|
Record($"self-right to upright {uprightDeg:F1} deg (limit {UprightLimitDeg})", uprightDeg < UprightLimitDeg);
|
|
Record($"yaw held through tumble {yawErr:F1} deg (limit {YawHoldLimitDeg})", yawErr < YawHoldLimitDeg);
|
|
Finish();
|
|
}
|
|
|
|
private static float YawErrorDeg(Basis bodyBasis, Basis headBasis)
|
|
{
|
|
Vector3 bodyFwd = -bodyBasis.Z;
|
|
Vector3 headFwd = -headBasis.Z;
|
|
bodyFwd.Y = 0f;
|
|
headFwd.Y = 0f;
|
|
if (bodyFwd.LengthSquared() < 0.0001f || headFwd.LengthSquared() < 0.0001f)
|
|
return 0f;
|
|
bodyFwd = bodyFwd.Normalized();
|
|
headFwd = headFwd.Normalized();
|
|
return Mathf.RadToDeg(Mathf.Atan2(bodyFwd.Cross(headFwd).Dot(Vector3.Up), bodyFwd.Dot(headFwd)));
|
|
}
|
|
|
|
private void Record(string name, bool ok)
|
|
{
|
|
_resultNames.Add(name);
|
|
_resultOk.Add(ok);
|
|
GD.Print($"BODY: {name} → {(ok ? "PASS" : "FAIL")}");
|
|
}
|
|
|
|
private void Finish()
|
|
{
|
|
bool overall = true;
|
|
foreach (bool ok in _resultOk)
|
|
overall &= ok;
|
|
GD.Print($"BODY: overall {(overall ? "PASS — physical torso functional on Box3D" : "CHECK — inspect failures above")}");
|
|
GetTree().Quit();
|
|
}
|
|
}
|