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,250 @@
|
||||
using Godot;
|
||||
|
||||
public partial class BodyRig : Node3D
|
||||
{
|
||||
private const float HeadBackOffset = 0.06f;
|
||||
private const float HeadSize = 0.15f;
|
||||
private const float TorsoDrop = 0.42f;
|
||||
private const float TorsoBackOffset = 0.06f;
|
||||
private const float ShoulderRise = 0.25f;
|
||||
private const float ReachBudgetMax = 0.25f;
|
||||
private const float ReachVerticalMin = -0.05f;
|
||||
private const float ReachVerticalMax = 0.12f;
|
||||
private const float ReachPushSpeed = 0.8f;
|
||||
private const float ReachDecaySpeed = 0.4f;
|
||||
private const float ReachDecayFactor = 0.92f;
|
||||
private const float WristOffset = 0.06f;
|
||||
private const float SoftIKFactor = 0.98f;
|
||||
private const float PoleHandWeight = 0.25f;
|
||||
private const float PoleSmoothRate = 10f;
|
||||
private const float TorsoYawHandsWeight = 0.6f;
|
||||
private const float TorsoYawForwardness = 0.3f;
|
||||
private const float TorsoYawSmoothRate = 12f;
|
||||
|
||||
private XRCamera3D _camera;
|
||||
private XRController3D _left;
|
||||
private XRController3D _right;
|
||||
|
||||
private MeshInstance3D _headMesh;
|
||||
private MeshInstance3D _torsoMesh;
|
||||
private readonly MeshInstance3D[] _upperArmMeshes = new MeshInstance3D[2];
|
||||
private readonly MeshInstance3D[] _forearmMeshes = new MeshInstance3D[2];
|
||||
|
||||
private bool _calibrated;
|
||||
private float _upperLen;
|
||||
private float _foreLen;
|
||||
private float _shoulderHalfWidth;
|
||||
private Vector3 _torsoOffset = Vector3.Zero;
|
||||
private Basis _yawBasis = Basis.Identity;
|
||||
private Basis _torsoBasis = Basis.Identity;
|
||||
private float _torsoYaw;
|
||||
private readonly Vector3[] _lastPole = new Vector3[2];
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
_camera = GetNode<XRCamera3D>("../XROrigin3D/XRCamera3D");
|
||||
_left = GetNode<XRController3D>("../XROrigin3D/LeftHand");
|
||||
_right = GetNode<XRController3D>("../XROrigin3D/RightHand");
|
||||
|
||||
_headMesh = MakeMesh(new BoxMesh { Size = new Vector3(HeadSize, HeadSize, HeadSize) }, new Color(0.8f, 0.8f, 0.8f));
|
||||
_torsoMesh = MakeMesh(new BoxMesh { Size = new Vector3(0.22f, 0.5f, 0.13f) }, new Color(0.4f, 0.45f, 0.55f));
|
||||
_upperArmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
|
||||
_upperArmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.07f, 0.3f, 0.07f) }, new Color(0.3f, 0.3f, 0.33f));
|
||||
_forearmMeshes[0] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));
|
||||
_forearmMeshes[1] = MakeMesh(new BoxMesh { Size = new Vector3(0.06f, 0.3f, 0.06f) }, new Color(0.3f, 0.3f, 0.33f));
|
||||
|
||||
SetRigVisible(false);
|
||||
}
|
||||
|
||||
private MeshInstance3D MakeMesh(Mesh mesh, Color color)
|
||||
{
|
||||
MeshInstance3D node = new() { Mesh = mesh };
|
||||
node.SetSurfaceOverrideMaterial(0, new StandardMaterial3D { AlbedoColor = color, Roughness = 0.6f });
|
||||
AddChild(node);
|
||||
return node;
|
||||
}
|
||||
|
||||
private void SetRigVisible(bool visible)
|
||||
{
|
||||
_headMesh.Visible = visible;
|
||||
_torsoMesh.Visible = visible;
|
||||
foreach (MeshInstance3D m in _upperArmMeshes)
|
||||
m.Visible = visible;
|
||||
foreach (MeshInstance3D m in _forearmMeshes)
|
||||
m.Visible = visible;
|
||||
}
|
||||
|
||||
public override void _Process(double delta)
|
||||
{
|
||||
UpdateYawBasis();
|
||||
float dt = (float)delta;
|
||||
|
||||
if (!_calibrated)
|
||||
{
|
||||
float headHeight = _camera.GlobalPosition.Y;
|
||||
if (headHeight <= 0.5f)
|
||||
return;
|
||||
Calibrate(headHeight);
|
||||
}
|
||||
|
||||
Vector3 headPos = _camera.GlobalPosition - _yawBasis * Vector3.Forward * HeadBackOffset;
|
||||
_headMesh.GlobalTransform = new Transform3D(_yawBasis, headPos);
|
||||
|
||||
UpdateTorsoYaw(headPos, dt);
|
||||
|
||||
Vector3 torsoBack = _torsoBasis * Vector3.Back;
|
||||
Vector3 torsoCenter = headPos + Vector3.Down * TorsoDrop + torsoBack * TorsoBackOffset + _torsoOffset;
|
||||
_torsoMesh.GlobalTransform = new Transform3D(_torsoBasis, torsoCenter);
|
||||
|
||||
Vector3 topCenter = torsoCenter + Vector3.Up * ShoulderRise;
|
||||
Vector3 leftShoulder = topCenter - _torsoBasis.X * _shoulderHalfWidth;
|
||||
Vector3 rightShoulder = topCenter + _torsoBasis.X * _shoulderHalfWidth;
|
||||
|
||||
Vector3 push = SolveArm(0, leftShoulder, -_torsoBasis.X, PhysicsHands.GetBody(_left), _upperArmMeshes[0], _forearmMeshes[0], dt, out bool decayLeft);
|
||||
push += SolveArm(1, rightShoulder, _torsoBasis.X, PhysicsHands.GetBody(_right), _upperArmMeshes[1], _forearmMeshes[1], dt, out bool decayRight);
|
||||
|
||||
if (push != Vector3.Zero)
|
||||
{
|
||||
_torsoOffset += push * (dt * ReachPushSpeed);
|
||||
}
|
||||
else if (decayLeft || decayRight)
|
||||
{
|
||||
_torsoOffset = _torsoOffset.MoveToward(Vector3.Zero, ReachDecaySpeed * dt);
|
||||
}
|
||||
|
||||
Vector3 horizontal = new(_torsoOffset.X, 0f, _torsoOffset.Z);
|
||||
horizontal = horizontal.LimitLength(ReachBudgetMax);
|
||||
_torsoOffset = new Vector3(horizontal.X, Mathf.Clamp(_torsoOffset.Y, ReachVerticalMin, ReachVerticalMax), horizontal.Z);
|
||||
}
|
||||
|
||||
private void UpdateYawBasis()
|
||||
{
|
||||
Vector3 fwd = -_camera.GlobalTransform.Basis.Z;
|
||||
fwd.Y = 0f;
|
||||
if (fwd.LengthSquared() < 0.0001f)
|
||||
return;
|
||||
_yawBasis = Basis.LookingAt(fwd.Normalized(), Vector3.Up);
|
||||
}
|
||||
|
||||
private void UpdateTorsoYaw(Vector3 headPos, float delta)
|
||||
{
|
||||
float hmdYaw = YawOf(-_yawBasis.Z);
|
||||
RigidBody3D leftBody = PhysicsHands.GetBody(_left);
|
||||
RigidBody3D rightBody = PhysicsHands.GetBody(_right);
|
||||
float targetYaw = hmdYaw;
|
||||
if (leftBody != null && rightBody != null)
|
||||
{
|
||||
Vector3 toHands = (leftBody.GlobalPosition + rightBody.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) / TorsoYawForwardness, 0f, 1f);
|
||||
targetYaw = Mathf.LerpAngle(hmdYaw, handsYaw, forwardness * TorsoYawHandsWeight);
|
||||
}
|
||||
}
|
||||
_torsoYaw = Mathf.LerpAngle(_torsoYaw, targetYaw, 1f - Mathf.Exp(-delta * TorsoYawSmoothRate));
|
||||
Vector3 fwd = new(-Mathf.Sin(_torsoYaw), 0f, -Mathf.Cos(_torsoYaw));
|
||||
_torsoBasis = Basis.LookingAt(fwd, Vector3.Up);
|
||||
}
|
||||
|
||||
private static float YawOf(Vector3 forward)
|
||||
{
|
||||
return Mathf.Atan2(-forward.X, -forward.Z);
|
||||
}
|
||||
|
||||
private void Calibrate(float headHeight)
|
||||
{
|
||||
_calibrated = true;
|
||||
_torsoYaw = YawOf(-_yawBasis.Z);
|
||||
float armTotal = Mathf.Clamp(headHeight * 0.46f, 0.5f, 0.72f);
|
||||
_upperLen = 0.5f * armTotal;
|
||||
_foreLen = 0.5f * armTotal;
|
||||
_shoulderHalfWidth = 0.16f + headHeight * 0.02f;
|
||||
|
||||
((BoxMesh)_upperArmMeshes[0].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f);
|
||||
((BoxMesh)_upperArmMeshes[1].Mesh).Size = new Vector3(0.07f, _upperLen, 0.07f);
|
||||
((BoxMesh)_forearmMeshes[0].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f);
|
||||
((BoxMesh)_forearmMeshes[1].Mesh).Size = new Vector3(0.06f, _foreLen, 0.06f);
|
||||
|
||||
SetRigVisible(true);
|
||||
GD.Print($"BodyRig: proportions calibrated from head height {headHeight:F2} m " +
|
||||
$"(arm total {armTotal:F2} m, upper {_upperLen:F2} m, fore {_foreLen:F2} m, shoulder half-width {_shoulderHalfWidth:F2} m)");
|
||||
}
|
||||
|
||||
private Vector3 SolveArm(int index, Vector3 shoulder, Vector3 side, RigidBody3D body, MeshInstance3D upperMesh, MeshInstance3D foreMesh, float delta, out bool decay)
|
||||
{
|
||||
Vector3 rawTarget = body != null
|
||||
? body.GlobalPosition + body.GlobalTransform.Basis * new Vector3(0f, 0f, WristOffset)
|
||||
: shoulder + _torsoBasis * new Vector3(0f, -0.25f, -0.3f);
|
||||
|
||||
float reach = _upperLen + _foreLen;
|
||||
Vector3 shoulderToTarget = rawTarget - shoulder;
|
||||
if (shoulderToTarget.LengthSquared() < 0.0001f)
|
||||
shoulderToTarget = _torsoBasis * new Vector3(0f, -0.25f, -0.3f);
|
||||
|
||||
float rawDist = shoulderToTarget.Length();
|
||||
float d = Mathf.Clamp(rawDist, 0.01f, reach * SoftIKFactor);
|
||||
Vector3 dir = shoulderToTarget / rawDist;
|
||||
Vector3 clampedTarget = shoulder + dir * d;
|
||||
|
||||
Vector3 push = Vector3.Zero;
|
||||
decay = false;
|
||||
if (rawDist > reach)
|
||||
{
|
||||
push = (rawTarget - clampedTarget).Normalized();
|
||||
}
|
||||
else if (rawDist < reach * ReachDecayFactor)
|
||||
{
|
||||
decay = true;
|
||||
}
|
||||
|
||||
float cosA = (_upperLen * _upperLen + d * d - _foreLen * _foreLen) / (2f * _upperLen * d);
|
||||
float angle = Mathf.Acos(Mathf.Clamp(cosA, -1f, 1f));
|
||||
|
||||
Vector3 handForward = body != null ? body.GlobalTransform.Basis * Vector3.Forward : Vector3.Zero;
|
||||
Vector3 pole = Vector3.Down * 2f + _torsoBasis * Vector3.Back * 0.8f + side * 0.3f;
|
||||
pole -= dir * pole.Dot(dir);
|
||||
pole -= handForward * PoleHandWeight;
|
||||
Vector3 poleDir;
|
||||
if (pole.LengthSquared() < 0.0001f)
|
||||
poleDir = _lastPole[index] == Vector3.Zero ? Vector3.Down : _lastPole[index];
|
||||
else
|
||||
{
|
||||
poleDir = _lastPole[index].Lerp(pole, 1f - Mathf.Exp(-delta * PoleSmoothRate));
|
||||
if (poleDir.LengthSquared() < 0.0001f)
|
||||
poleDir = _lastPole[index];
|
||||
poleDir = poleDir.Normalized();
|
||||
_lastPole[index] = poleDir;
|
||||
}
|
||||
|
||||
Vector3 axis = dir.Cross(poleDir);
|
||||
if (axis.LengthSquared() < 0.0001f)
|
||||
axis = dir.Cross(Vector3.Up);
|
||||
if (axis.LengthSquared() < 0.0001f)
|
||||
axis = Vector3.Right;
|
||||
axis = axis.Normalized();
|
||||
|
||||
Vector3 elbowPos = shoulder + dir.Rotated(axis, angle) * _upperLen;
|
||||
Vector3 foreEnd = elbowPos + (clampedTarget - elbowPos).Normalized() * _foreLen;
|
||||
|
||||
PlaceSegment(upperMesh, shoulder, elbowPos);
|
||||
PlaceSegment(foreMesh, elbowPos, foreEnd);
|
||||
|
||||
return push;
|
||||
}
|
||||
|
||||
private static void PlaceSegment(MeshInstance3D mesh, Vector3 from, Vector3 to)
|
||||
{
|
||||
Vector3 segment = to - from;
|
||||
if (segment.LengthSquared() < 0.000001f)
|
||||
return;
|
||||
Vector3 y = segment.Normalized();
|
||||
Vector3 x = Vector3.Up.Cross(y);
|
||||
if (x.LengthSquared() < 0.0001f)
|
||||
x = Vector3.Right.Cross(y);
|
||||
x = x.Normalized();
|
||||
Vector3 z = x.Cross(y);
|
||||
mesh.GlobalTransform = new Transform3D(new Basis(x, y, z), (from + to) * 0.5f);
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user