296 lines
13 KiB
C#
296 lines
13 KiB
C#
using Godot;
|
|
|
|
// In-game driver of the ragdoll player (body step 2): XR input → True Body → PlayerRagdoll, plus
|
|
// Boneworks-style stick locomotion and the play-space reconciliation. Runs everything in one
|
|
// physics tick, in this order:
|
|
// 1. discrete origin jumps made by other code (spawn, teleport markers) carry the body along;
|
|
// 2. stick: left = move (head-relative), right X = turn (snap by default, SmoothTurn for smooth),
|
|
// right Y = crouch (down lowers, up raises — RagdollDriver.StickCrouch); right A = hold to charge, release to jump;
|
|
// 3. the ragdoll's OriginCorrection (push-back at walls, floor follows the ground, falls);
|
|
// 4. True Body solve from the corrected camera/controllers, then the ragdoll's muscles.
|
|
// Waits for the calibrated BodyProfile like everything else; rebuilds on recalibration.
|
|
public partial class PlayerRig : Node
|
|
{
|
|
public const float MoveSpeed = 2.2f; // m/s at full stick (Boneworks-ish walk)
|
|
public const float MoveAcceleration = 8f; // m/s²
|
|
public const float StickDeadzone = 0.2f;
|
|
public static bool SmoothTurn = false;
|
|
// Tracking timed by the render clock (bible v2: uniform motion, constant ~2-tick lag) vs by
|
|
// arrival (less lag, 18 Hz speed beat). Menu toggle so both can be compared in the headset.
|
|
public static bool SteadyTracking = true;
|
|
public const float SnapTurnDegrees = 30f;
|
|
public const float SmoothTurnSpeed = 2.5f; // rad/s
|
|
public const float SnapTurnThreshold = 0.7f;
|
|
public const float SnapTurnRearm = 0.4f;
|
|
public const float StickCrouchSpeed = 1.2f; // m/s at full stick
|
|
public const float StickCrouchHeadroom = 0.5f; // the stick can lower the eyes to this height (a crawl)
|
|
public const float JumpDistance = 0.5f; // an origin move this big in one tick = teleport, not motion
|
|
public const float JumpAngle = 0.1f; // rad
|
|
private const float GhostLagDistance = 0.1f;
|
|
|
|
private static PlayerRig _instance;
|
|
|
|
private XROrigin3D _origin;
|
|
private XRCamera3D _camera;
|
|
private XRController3D _left, _right;
|
|
private TrueBody _trueBody;
|
|
private PlayerRagdoll _ragdoll;
|
|
private Transform3D _lastOrigin;
|
|
private bool _haveLastOrigin;
|
|
private Vector3 _moveVelocity;
|
|
private bool _snapArmed = true;
|
|
private readonly MeshInstance3D[] _ghosts = new MeshInstance3D[2];
|
|
private bool _rebuild = true;
|
|
private RagdollDriver _driver;
|
|
// Render interpolation of the play space (smooth-camera bible, VR edition): physics moves the
|
|
// origin every tick, but the body is DRAWN interpolated between the last two ticks. A camera
|
|
// stepping with the raw origin while the body glides a tick behind looks like the body is in
|
|
// two places at once when you move and look at it. So physics owns _physOrigin and every
|
|
// frame the visible origin is the same interpolation of it the bodies get.
|
|
private Transform3D _physOrigin, _prevPhysOrigin, _shownOrigin;
|
|
private bool _interpolateOrigin;
|
|
|
|
public PlayerRagdoll Ragdoll => _ragdoll;
|
|
public static bool Spectator { get; private set; }
|
|
|
|
// Spectator = no body (fly with the old grab-drag movement); leaving it rebuilds the body at
|
|
// the current pose.
|
|
public static void SetSpectator(bool on)
|
|
{
|
|
Spectator = on;
|
|
AnchorMoveMode.Enabled = on;
|
|
if (_instance == null)
|
|
return;
|
|
if (on)
|
|
_instance._ragdoll.Clear();
|
|
_instance._rebuild = true;
|
|
GD.Print($"PlayerRig: spectator {(on ? "on" : "off")}");
|
|
}
|
|
|
|
public static RigidBody3D GetHandBody(XRController3D controller)
|
|
{
|
|
if (_instance == null || !_instance._ragdoll.Built)
|
|
return null;
|
|
return _instance._ragdoll.Hand(controller == _instance._left ? 0 : 1);
|
|
}
|
|
|
|
public static void SetHeld(XRController3D controller, PhysicsBody3D prop)
|
|
{
|
|
if (_instance != null)
|
|
_instance._ragdoll.SetHeld(controller == _instance._left ? 0 : 1, prop);
|
|
}
|
|
|
|
public override void _Ready()
|
|
{
|
|
_instance = this;
|
|
ProcessPriority = -100; // place the interpolated origin before anything reads it this frame
|
|
ProcessPhysicsPriority = -100; // restore the physics origin before anything uses it this tick
|
|
_origin = GetNode<XROrigin3D>("../XROrigin3D");
|
|
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
|
|
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
|
|
_right = GetNode<XRController3D>("../XROrigin3D/RightHand");
|
|
_trueBody = GetNodeOrNull<TrueBody>("../TrueBody");
|
|
if (_trueBody != null)
|
|
{
|
|
_trueBody.ExternallySolved = true;
|
|
TrueBody.ShowDebug = false;
|
|
}
|
|
// PlayerRig itself is excluded from physics interpolation (ghosts follow controllers per
|
|
// frame); the body is simulated, so it interpolates.
|
|
_ragdoll = new PlayerRagdoll { Name = "Ragdoll", PhysicsInterpolationMode = PhysicsInterpolationModeEnum.On };
|
|
AddChild(_ragdoll);
|
|
_driver = new RagdollDriver { Solver = Solver, Ragdoll = _ragdoll };
|
|
BodyProfile.Changed += _ => _rebuild = true;
|
|
for (int i = 0; i < 2; i++)
|
|
{
|
|
_ghosts[i] = new MeshInstance3D
|
|
{
|
|
Mesh = new BoxMesh { Size = PlayerRagdoll.HandSize },
|
|
Visible = false,
|
|
CastShadow = GeometryInstance3D.ShadowCastingSetting.Off,
|
|
};
|
|
_ghosts[i].SetSurfaceOverrideMaterial(0, new StandardMaterial3D
|
|
{
|
|
AlbedoColor = new Color(1f, 1f, 1f, 0.3f),
|
|
Transparency = BaseMaterial3D.TransparencyEnum.Alpha,
|
|
ShadingMode = BaseMaterial3D.ShadingModeEnum.Unshaded,
|
|
});
|
|
AddChild(_ghosts[i]);
|
|
}
|
|
}
|
|
|
|
private TrueBodySolver Solver => _trueBody != null ? _trueBody.Solver : _ownSolver;
|
|
private readonly TrueBodySolver _ownSolver = new();
|
|
|
|
public override void _PhysicsProcess(double delta)
|
|
{
|
|
float dt = (float)delta;
|
|
BodyProfile profile = BodyProfile.Current;
|
|
if (profile == null || Spectator)
|
|
{
|
|
_haveLastOrigin = false;
|
|
_interpolateOrigin = false;
|
|
return;
|
|
}
|
|
RestorePhysicsOrigin();
|
|
|
|
// 1. Someone else moved the play space discretely: bring the body along.
|
|
Transform3D origin = _origin.GlobalTransform;
|
|
if (_haveLastOrigin && _ragdoll.Built && IsJump(_lastOrigin, origin))
|
|
Carry(origin * _lastOrigin.AffineInverse());
|
|
|
|
// 2. Stick locomotion.
|
|
if (_ragdoll.Built && !_driver.RebuildRequested)
|
|
Locomote(dt);
|
|
|
|
// 3. Filtered tracking → play-space correction → True Body → muscles (shared with RagdollSpike).
|
|
_driver.RebuildRequested |= _rebuild;
|
|
_rebuild = false;
|
|
Transform3D o = _origin.GlobalTransform;
|
|
_driver.Tick(profile, ref o, _camera.Transform, _left.Transform, _right.Transform, _moveVelocity, dt,
|
|
SteadyTracking ? TrackingAge() : null);
|
|
_origin.GlobalTransform = o;
|
|
_lastOrigin = _origin.GlobalTransform;
|
|
_haveLastOrigin = true;
|
|
_prevPhysOrigin = _interpolateOrigin && !IsJump(_physOrigin, _lastOrigin) ? _physOrigin : _lastOrigin;
|
|
_physOrigin = _shownOrigin = _lastOrigin;
|
|
_interpolateOrigin = true;
|
|
}
|
|
|
|
// The tracking poses read in this tick were delivered with the last frame, predicted for its render
|
|
// time. In physics ticks: render time of that frame ≈ (physics frames + fraction) at its _process,
|
|
// plus one frame delta for the frame these ticks belong to (bible v2 recipe).
|
|
private double TrackingAge()
|
|
{
|
|
float rate = Engine.PhysicsTicksPerSecond;
|
|
ulong frame = Engine.GetProcessFrames();
|
|
if (frame != _sampleFrame)
|
|
{
|
|
_sampleFrame = frame;
|
|
_sampleTicks = _renderTicks + _renderDelta * rate;
|
|
}
|
|
return (Engine.GetPhysicsFrames() - _sampleTicks) / rate;
|
|
}
|
|
|
|
private double _renderTicks, _renderDelta, _sampleTicks;
|
|
private ulong _sampleFrame = ulong.MaxValue;
|
|
|
|
// The origin on screen is interpolated; physics must run on the real one. Anyone else who moved
|
|
// the origin since (teleport marker, menu) is respected as the new real one.
|
|
private void RestorePhysicsOrigin()
|
|
{
|
|
if (!_interpolateOrigin)
|
|
return;
|
|
Transform3D current = _origin.GlobalTransform;
|
|
if (current.IsEqualApprox(_shownOrigin))
|
|
_origin.GlobalTransform = _physOrigin;
|
|
else
|
|
_physOrigin = _prevPhysOrigin = current;
|
|
}
|
|
|
|
public override void _Process(double delta)
|
|
{
|
|
_renderTicks = Engine.GetPhysicsFrames() + Engine.GetPhysicsInterpolationFraction();
|
|
_renderDelta = delta;
|
|
if (_interpolateOrigin && !Spectator)
|
|
{
|
|
Transform3D current = _origin.GlobalTransform;
|
|
if (!current.IsEqualApprox(_shownOrigin))
|
|
_physOrigin = _prevPhysOrigin = current; // moved by someone else: show it as is
|
|
_shownOrigin = _prevPhysOrigin.InterpolateWith(_physOrigin, (float)Engine.GetPhysicsInterpolationFraction());
|
|
_origin.GlobalTransform = _shownOrigin;
|
|
}
|
|
for (int i = 0; i < 2; i++)
|
|
{
|
|
XRController3D c = i == 0 ? _left : _right;
|
|
RigidBody3D hand = _ragdoll.Built ? _ragdoll.Hand(i) : null;
|
|
bool lagging = hand != null && hand.GlobalPosition.DistanceTo(c.GlobalPosition) > GhostLagDistance;
|
|
_ghosts[i].Visible = lagging;
|
|
if (lagging)
|
|
_ghosts[i].GlobalTransform = c.GlobalTransform;
|
|
}
|
|
}
|
|
|
|
// The body is held back (hips against a table, a wall the head clears): stop the stick from
|
|
// carrying the view further that way.
|
|
public static Vector3 HoldBack(Vector3 velocity, Vector3 heldBack)
|
|
{
|
|
float into = velocity.Dot(heldBack);
|
|
return into > 0f ? velocity - heldBack * into : velocity;
|
|
}
|
|
|
|
private static bool IsJump(Transform3D a, Transform3D b) =>
|
|
a.Origin.DistanceTo(b.Origin) > JumpDistance ||
|
|
BodyMath.RotationError(b.Basis.GetRotationQuaternion(), a.Basis.GetRotationQuaternion()).Length() > JumpAngle;
|
|
|
|
private void Carry(Transform3D delta)
|
|
{
|
|
_ragdoll.TeleportBy(delta);
|
|
Solver.Shift(delta);
|
|
}
|
|
|
|
private void Locomote(float dt)
|
|
{
|
|
// A stick whose hand points at the menu scrolls it instead.
|
|
Vector2 move = WorldPanel.IsPointing(_left) ? Vector2.Zero : _left.GetVector2("primary");
|
|
Vector2 turn = WorldPanel.IsPointing(_right) ? Vector2.Zero : _right.GetVector2("primary");
|
|
|
|
// Move: head-relative on the ground plane, acceleration-limited. The body follows the
|
|
// camera physically, so a wall stops it and the push-back stops the view.
|
|
Vector3 desired = Vector3.Zero;
|
|
if (move.Length() > StickDeadzone)
|
|
{
|
|
Vector3 fwd = BodyMath.HeadFacing(_camera.GlobalTransform.Basis.Orthonormalized());
|
|
if (fwd == Vector3.Zero)
|
|
fwd = -Solver.YawBasis.Z;
|
|
Vector3 rightDir = fwd.Cross(Vector3.Up);
|
|
float mag = Mathf.Clamp((move.Length() - StickDeadzone) / (1f - StickDeadzone), 0f, 1f);
|
|
desired = (fwd * move.Y + rightDir * move.X).Normalized() * (mag * MoveSpeed);
|
|
}
|
|
if (!_ragdoll.Grounded)
|
|
desired = _moveVelocity; // no air control
|
|
_moveVelocity = _moveVelocity.MoveToward(desired, MoveAcceleration * dt);
|
|
_moveVelocity = HoldBack(_moveVelocity, _ragdoll.HeldBack);
|
|
if (_moveVelocity != Vector3.Zero)
|
|
_origin.GlobalPosition += _moveVelocity * dt;
|
|
|
|
// Jump: hold right A to charge (the legs bend), release to jump.
|
|
_ragdoll.JumpHeld = _right.IsButtonPressed("ax_button");
|
|
|
|
// Crouch: right stick held down lowers the body, up raises it back (the stick's dominant
|
|
// axis decides between crouch and turn).
|
|
bool vertical = Mathf.Abs(turn.Y) > Mathf.Abs(turn.X);
|
|
if (vertical && Mathf.Abs(turn.Y) > StickDeadzone)
|
|
{
|
|
float max = Mathf.Max(0f, BodyProfile.Current.EyeHeight - StickCrouchHeadroom);
|
|
_driver.StickCrouch = Mathf.Clamp(_driver.StickCrouch - turn.Y * StickCrouchSpeed * dt, 0f, max);
|
|
}
|
|
if (vertical)
|
|
turn.X = 0f;
|
|
|
|
// Turn about the head.
|
|
float angle = 0f;
|
|
if (SmoothTurn)
|
|
{
|
|
if (Mathf.Abs(turn.X) > StickDeadzone)
|
|
angle = -turn.X * SmoothTurnSpeed * dt;
|
|
}
|
|
else if (_snapArmed && Mathf.Abs(turn.X) > SnapTurnThreshold)
|
|
{
|
|
angle = -Mathf.Sign(turn.X) * Mathf.DegToRad(SnapTurnDegrees);
|
|
_snapArmed = false;
|
|
}
|
|
else if (Mathf.Abs(turn.X) < SnapTurnRearm)
|
|
_snapArmed = true;
|
|
if (angle != 0f)
|
|
{
|
|
Vector3 pivot = _camera.GlobalPosition;
|
|
Transform3D rot = new Transform3D(Basis.Identity, pivot) * new Transform3D(new Basis(Vector3.Up, angle), Vector3.Zero)
|
|
* new Transform3D(Basis.Identity, -pivot);
|
|
_origin.GlobalTransform = rot * _origin.GlobalTransform;
|
|
if (!SmoothTurn)
|
|
Carry(rot);
|
|
}
|
|
}
|
|
}
|