111 lines
6.1 KiB
C#
111 lines
6.1 KiB
C#
using Godot;
|
||
|
||
// Analytic 2-bone arm solve shared by PhysicsArms and TrueBody (one instance per arm — it keeps
|
||
// the elbow's smoothed pole angle). Soft-IK clamp, then the elbow is picked on its circle as an
|
||
// ANGLE around the arm axis relative to a torso-frame down/back/out reference: mostly what the
|
||
// hand's forward axis suggests (the forearm continues behind the wrist), clamped to ±110° and
|
||
// rate-limited so it can never swing through the inward/up side (the 180° flip), and capped
|
||
// at max(shoulder, wrist) + 5 cm height (no chicken wings).
|
||
public sealed class ArmSolver
|
||
{
|
||
public const float SoftIKFactor = 0.98f;
|
||
public const float PoleHandWeight = 0.85f;
|
||
public const float PoleSmoothRate = 12f;
|
||
public const float PoleMaxAngle = 1.92f; // 110 deg outward from down/back/out
|
||
public const float PoleMaxInward = 0.6f; // 35 deg inward: a controller turned outward must not fold the elbows toward each other (owner)
|
||
public const float PoleMaxSpeed = 6f; // rad/s
|
||
public const float ElbowRiseAllowance = 0.05f;
|
||
|
||
private float _poleAngle;
|
||
private Vector3 _lastRef;
|
||
|
||
public Vector3 Elbow { get; private set; }
|
||
public Vector3 Wrist { get; private set; }
|
||
// Unit vector from the arm axis toward the elbow (defined even for a straight arm); the
|
||
// elbow hinge axis is Pole × (shoulder→wrist).
|
||
public Vector3 Pole { get; private set; } = Vector3.Back;
|
||
|
||
// side = the arm's outward direction (−torso X for the left arm, +X for the right).
|
||
// handBasis = the controller's basis (null → torso-frame reference only).
|
||
public void Solve(Vector3 shoulder, Vector3 rawTarget, Basis torsoBasis, Vector3 side, Basis? handBasis,
|
||
float upperLen, float foreLen, float delta)
|
||
{
|
||
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();
|
||
// Below |upper − fore| the triangle can't close (law of cosines invalid) — with unequal
|
||
// anatomical segments a hand near the shoulder would otherwise throw the elbow away.
|
||
float d = Mathf.Clamp(rawDist, Mathf.Abs(upperLen - foreLen) + 0.01f, reach * SoftIKFactor);
|
||
Vector3 dir = shoulderToTarget / rawDist;
|
||
Vector3 wrist = shoulder + dir * d;
|
||
|
||
// Elbow circle: center on the arm axis, radius from the law of cosines.
|
||
float along = (upperLen * upperLen - foreLen * foreLen + d * d) / (2f * d);
|
||
float radius = Mathf.Sqrt(Mathf.Max(upperLen * upperLen - along * along, 0f));
|
||
Vector3 center = shoulder + dir * along;
|
||
|
||
Vector3 reference = Vector3.Down * 2f + torsoBasis * Vector3.Back * 0.8f + side * 0.3f;
|
||
reference -= dir * reference.Dot(dir);
|
||
if (reference.LengthSquared() > 0.06f)
|
||
_lastRef = reference.Normalized();
|
||
else
|
||
{
|
||
// Arm pointing along the reference: keep the previous one, re-projected.
|
||
reference = _lastRef - dir * _lastRef.Dot(dir);
|
||
reference = reference.LengthSquared() > 1e-8f ? reference.Normalized() : dir.Cross(torsoBasis.X).Normalized();
|
||
}
|
||
Vector3 refN = reference.Normalized();
|
||
Vector3 refB = dir.Cross(refN);
|
||
|
||
float targetAngle = 0f;
|
||
if (handBasis is Basis hb)
|
||
{
|
||
Vector3 handForward = (hb * Vector3.Forward).Normalized();
|
||
Vector3 handElbow = wrist - handForward * foreLen - center;
|
||
handElbow -= dir * handElbow.Dot(dir);
|
||
float confidence = Mathf.Clamp(handElbow.Length() / (0.5f * radius + 1e-4f), 0f, 1f);
|
||
if (handElbow.LengthSquared() > 1e-8f)
|
||
{
|
||
// Read the suggestion as the representation NEAREST the current elbow angle:
|
||
// pointing the controller down puts it at the ±180° seam, where jitter would
|
||
// otherwise flip the clamped elbow to the other side.
|
||
float raw = Mathf.Atan2(handElbow.Dot(refB), handElbow.Dot(refN));
|
||
raw = _poleAngle + Mathf.Wrap(raw - _poleAngle, -Mathf.Pi, Mathf.Pi);
|
||
targetAngle = raw * PoleHandWeight * confidence;
|
||
}
|
||
}
|
||
// Asymmetric: the elbow may swing far out, but only a little in toward the body's midline.
|
||
float outward = refB.Dot(side); // > 0: a positive angle swings the elbow outward
|
||
float inwardLimit = Mathf.Lerp(PoleMaxAngle, PoleMaxInward, Mathf.Clamp(Mathf.Abs(outward) / 0.5f, 0f, 1f));
|
||
targetAngle = outward >= 0f ? Mathf.Clamp(targetAngle, -inwardLimit, PoleMaxAngle) : Mathf.Clamp(targetAngle, -PoleMaxAngle, inwardLimit);
|
||
float step = (targetAngle - _poleAngle) * (1f - Mathf.Exp(-delta * PoleSmoothRate));
|
||
_poleAngle += Mathf.Clamp(step, -PoleMaxSpeed * delta, PoleMaxSpeed * delta);
|
||
Vector3 pole = refN.Rotated(dir, _poleAngle);
|
||
|
||
Pole = CapElbowHeight(pole, dir, center, radius, Mathf.Max(shoulder.Y, wrist.Y) + ElbowRiseAllowance);
|
||
Elbow = center + Pole * radius;
|
||
Wrist = Elbow + (wrist - Elbow).Normalized() * foreLen;
|
||
}
|
||
|
||
// A real wrist flexes, so following the hand axis literally can throw the elbow above
|
||
// the shoulder ("chicken wing"). Keep the elbow no higher than yMax by sliding it down
|
||
// its circle, preserving its sideways heading.
|
||
private static Vector3 CapElbowHeight(Vector3 pole, Vector3 dir, Vector3 center, float radius, float yMax)
|
||
{
|
||
if (center.Y + pole.Y * radius <= yMax)
|
||
return pole;
|
||
Vector3 vertical = Vector3.Up - dir * dir.Y;
|
||
if (vertical.LengthSquared() < 1e-6f)
|
||
return pole; // arm vertical: the circle is horizontal, height can't change
|
||
vertical = vertical.Normalized();
|
||
Vector3 lateral = dir.Cross(vertical);
|
||
float cosMax = Mathf.Clamp((yMax - center.Y) / (radius * vertical.Y), -1f, 1f);
|
||
float sin = Mathf.Sqrt(1f - cosMax * cosMax);
|
||
float sideSign = pole.Dot(lateral) >= 0f ? 1f : -1f;
|
||
return (vertical * cosMax + lateral * (sin * sideSign)).Normalized();
|
||
}
|
||
}
|