Body physics Stage 1: PhysicsTorso chase chest (30 kg PD + yaw servo), collision layers reworked (3=body, 4=hands, no self-collision), BodyRig visual-only, BodySpike regression (PASS 3x, all spikes green)
This commit is contained in:
Foxyz1302 committed
1 parent
d46a742b66
commit
4fdfcfd928
11 files changed
+586
-83
No files matched your search
@@ -0,0 +1,165 @@
|
||||
using Godot;
|
||||
|
||||
public partial class PhysicsTorso : Node
|
||||
{
|
||||
private const float TorsoMass = 30f;
|
||||
private const float MaxForce = 2400f;
|
||||
private const float MaxSpeed = 6f;
|
||||
public const float SnapDistance = 2f;
|
||||
private const float TorsoDrop = 0.42f;
|
||||
private const float TorsoBackOffset = 0.06f;
|
||||
private const float HeadBackOffset = 0.06f;
|
||||
public const float YawHandsWeight = 0.6f;
|
||||
public const float YawForwardness = 0.3f;
|
||||
public const float YawSmoothRate = 12f;
|
||||
private const float OrientKp = 20f;
|
||||
private const float OrientMaxSpin = 10f;
|
||||
private const float OrientMaxAlpha = 150f;
|
||||
|
||||
private static readonly Vector3 TorsoSize = new(0.22f, 0.5f, 0.13f);
|
||||
|
||||
private static readonly Color BodyColor = new(0.4f, 0.45f, 0.55f);
|
||||
|
||||
public Node3D HeadSource;
|
||||
public RigidBody3D HandBodyLeft;
|
||||
public RigidBody3D HandBodyRight;
|
||||
public Vector3 ReachOffset;
|
||||
|
||||
private XRCamera3D _camera;
|
||||
private XRController3D _leftController;
|
||||
private XRController3D _rightController;
|
||||
private RigidBody3D _handL;
|
||||
private RigidBody3D _handR;
|
||||
private RigidBody3D _body;
|
||||
|
||||
private Basis _yawBasis = Basis.Identity;
|
||||
private Basis _torsoBasis = Basis.Identity;
|
||||
private float _torsoYaw;
|
||||
private bool _yawInitialized;
|
||||
private bool _arrived;
|
||||
private MeshInstance3D _mesh;
|
||||
|
||||
public RigidBody3D Body => _body;
|
||||
public Basis TorsoBasis => _body != null ? _body.GlobalTransform.Basis : _torsoBasis;
|
||||
public Vector3 TargetPosition { get; private set; }
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
_body = new RigidBody3D
|
||||
{
|
||||
Name = "TorsoBody",
|
||||
Mass = TorsoMass,
|
||||
GravityScale = 0f,
|
||||
ContinuousCd = true,
|
||||
ContactMonitor = true,
|
||||
MaxContactsReported = 8,
|
||||
CollisionLayer = 0b100, // layer 3 = player body
|
||||
CollisionMask = 0b101, // world + player body; hands (layer 4) excluded both ways
|
||||
};
|
||||
_body.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = TorsoSize } });
|
||||
_mesh = new MeshInstance3D { Mesh = new BoxMesh { Size = TorsoSize }, Visible = false };
|
||||
_mesh.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = BodyColor, Roughness = 0.6f });
|
||||
_body.AddChild(_mesh);
|
||||
AddChild(_body);
|
||||
|
||||
if (HeadSource == null)
|
||||
{
|
||||
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
|
||||
_leftController = GetNode<XRController3D>("../XROrigin3D/LeftHand");
|
||||
_rightController = GetNode<XRController3D>("../XROrigin3D/RightHand");
|
||||
_handL = PhysicsHands.GetBody(_leftController);
|
||||
_handR = PhysicsHands.GetBody(_rightController);
|
||||
}
|
||||
GD.Print("PhysicsTorso: chase body spawned");
|
||||
}
|
||||
|
||||
public override void _PhysicsProcess(double delta)
|
||||
{
|
||||
Node3D head = HeadSource != null ? HeadSource : (Node3D)_camera;
|
||||
if (head == null || _body == null)
|
||||
return;
|
||||
float dt = (float)delta;
|
||||
|
||||
_yawBasis = FlatYawBasis(head.GlobalTransform.Basis);
|
||||
Vector3 headPos = head.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
|
||||
UpdateTorsoYaw(headPos, dt);
|
||||
Vector3 target = headPos + _torsoBasis * new Vector3(0f, -TorsoDrop, TorsoBackOffset) + ReachOffset;
|
||||
TargetPosition = target;
|
||||
|
||||
if (!_arrived && _body.GlobalPosition.DistanceTo(target) < 0.3f)
|
||||
{
|
||||
_arrived = true;
|
||||
_mesh.Visible = true;
|
||||
}
|
||||
|
||||
if (_body.GlobalPosition.DistanceTo(target) > SnapDistance)
|
||||
{
|
||||
_body.GlobalTransform = new Transform3D(_torsoBasis, target);
|
||||
_body.LinearVelocity = Vector3.Zero;
|
||||
_body.AngularVelocity = Vector3.Zero;
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 desiredVelocity = (target - _body.GlobalPosition) / dt;
|
||||
if (desiredVelocity.Length() > MaxSpeed)
|
||||
desiredVelocity = desiredVelocity.Normalized() * MaxSpeed;
|
||||
Vector3 dv = desiredVelocity - _body.LinearVelocity;
|
||||
Vector3 force = dv / dt * TorsoMass;
|
||||
if (force.Length() > MaxForce)
|
||||
force = force.Normalized() * MaxForce;
|
||||
_body.LinearVelocity += force / TorsoMass * dt;
|
||||
|
||||
Quaternion qErr = _torsoBasis.GetRotationQuaternion()
|
||||
* _body.GlobalTransform.Basis.GetRotationQuaternion().Inverse();
|
||||
if (qErr.W < 0f)
|
||||
qErr = new Quaternion(-qErr.X, -qErr.Y, -qErr.Z, -qErr.W);
|
||||
float angle = qErr.GetAngle();
|
||||
Vector3 spinAxis = angle > 1e-5f ? qErr.GetAxis() : Vector3.Up;
|
||||
Vector3 spinDesired = spinAxis * Mathf.Min(angle * OrientKp, OrientMaxSpin);
|
||||
|
||||
Vector3 wErr = spinDesired - _body.AngularVelocity;
|
||||
float maxDeltaW = OrientMaxAlpha * dt;
|
||||
if (wErr.Length() > maxDeltaW)
|
||||
wErr = wErr.Normalized() * maxDeltaW;
|
||||
_body.AngularVelocity += wErr;
|
||||
}
|
||||
|
||||
private void UpdateTorsoYaw(Vector3 headPos, float delta)
|
||||
{
|
||||
float hmdYaw = YawOf(-_yawBasis.Z);
|
||||
float targetYaw = hmdYaw;
|
||||
if (_handL != null && _handR != null)
|
||||
{
|
||||
Vector3 toHands = (_handL.GlobalPosition + _handR.GlobalPosition) * 0.5f - headPos;
|
||||
Vector3 horizontal = new(toHands.X, 0f, toHands.Z);
|
||||
if (horizontal.LengthSquared() > 0.0001f)
|
||||
{
|
||||
float handsYaw = YawOf(horizontal.Normalized());
|
||||
float forwardness = Mathf.Clamp(horizontal.Dot(-_yawBasis.Z) / YawForwardness, 0f, 1f);
|
||||
targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * YawHandsWeight);
|
||||
}
|
||||
}
|
||||
if (!_yawInitialized)
|
||||
{
|
||||
_torsoYaw = hmdYaw;
|
||||
_yawInitialized = true;
|
||||
}
|
||||
_torsoYaw = Mathf.LerpAngle(_torsoYaw, targetYaw, 1f - Mathf.Exp(-delta * YawSmoothRate));
|
||||
Vector3 fwd = new(-Mathf.Sin(_torsoYaw), 0f, -Mathf.Cos(_torsoYaw));
|
||||
_torsoBasis = Basis.LookingAt(fwd, Vector3.Up);
|
||||
}
|
||||
|
||||
private static Basis FlatYawBasis(Basis basis)
|
||||
{
|
||||
Vector3 fwd = -basis.Z;
|
||||
fwd.Y = 0f;
|
||||
if (fwd.LengthSquared() < 0.0001f)
|
||||
return Basis.Identity;
|
||||
return Basis.LookingAt(fwd.Normalized(), Vector3.Up);
|
||||
}
|
||||
|
||||
private static float YawOf(Vector3 forward)
|
||||
{
|
||||
return Mathf.Atan2(-forward.X, -forward.Z);
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user