Editor-side analytical two-bone arm IK for the weapon importer. Solves upper arm and forearm rotations so the hand reaches a target, keeps the elbow in the animation's bend plane, blends by weight, and applies stored hand finger rotations to a character pose.
#nullable enable annotations
using System.Numerics;
using WeaponImporter.EditorTools.Core.Hands;
using WeaponImporter.EditorTools.Core.Maths;
using WeaponImporter.EditorTools.Core.Rig;
namespace WeaponImporter.EditorTools.Core.Ik;
using Vector3 = System.Numerics.Vector3;
/// <summary>
/// Analytic two-bone arm IK. The elbow stays in the plane it already had in the animation, so
/// the solve makes the smallest change that gets the wrist onto its target.
/// </summary>
public static class ArmIk
{
/// <summary>
/// Moves upper arm and forearm so the hand reaches <paramref name="target"/>, blending by
/// <paramref name="weight"/>. The hand takes the target rotation. Returns reach info.
/// </summary>
public static ReachResult Solve(CharacterPose pose, HandRig arm, XForm target, float weight = 1f)
{
if (weight <= 1e-4f)
return new ReachResult(true, 0f, 0f);
var world = pose.World;
var upper = world[arm.UpperArm];
var lower = world[arm.LowerArm];
var hand = world[arm.Hand];
var desired = weight >= 0.9999f ? target : new XForm(Vector3.Lerp(hand.Pos, target.Pos, weight), MathQ.Slerp(hand.Rot, target.Rot, weight));
var s = upper.Pos;
var e = lower.Pos;
var w = hand.Pos;
var a = Vector3.Distance(s, e);
var b = Vector3.Distance(e, w);
var toTarget = desired.Pos - s;
var dist = toTarget.Length();
var reach = a + b;
var shortfall = MathF.Max(0f, dist - reach * 0.999f);
var c = Math.Clamp(dist, MathF.Abs(a - b) + 1e-3f, reach * 0.999f);
var d = dist > 1e-5f ? toTarget / dist : Vector3.Normalize(w - s);
// Bend direction: the animation's own bend (elbow offset from the shoulder->animated-wrist
// line), so it never flips when the target swings past the elbow. Nearly straight arms
// ease toward a relaxed down-and-back elbow.
var animReach = w - s;
var animPole = animReach.LengthSquared() > 1e-8f ? (e - s) - Vector3.Normalize(animReach) * Vector3.Dot(e - s, Vector3.Normalize(animReach)) : Vector3.Zero;
var relaxed = Vector3.Normalize(new Vector3(-0.2f, 0f, -0.8f));
var straightness = Math.Clamp(animPole.Length() / ((a + b) * 0.08f), 0f, 1f);
var bend = (animPole.LengthSquared() > 1e-10f ? Vector3.Normalize(animPole) : relaxed) * straightness + relaxed * (1f - straightness);
bend -= d * Vector3.Dot(bend, d);
if (bend.LengthSquared() < 1e-8f)
{
bend = Vector3.Cross(d, Vector3.Cross(-Vector3.UnitZ, d));
if (bend.LengthSquared() < 1e-8f)
bend = MathQ.Perpendicular(d);
}
bend = Vector3.Normalize(bend);
var cosA = Math.Clamp((a * a + c * c - b * b) / (2f * a * c), -1f, 1f);
var sinA = MathF.Sqrt(MathF.Max(0f, 1f - cosA * cosA));
var newElbow = s + (d * cosA + bend * sinA) * a;
// Rotate the upper arm so the elbow lands on its new spot.
var upperDelta = MathQ.FromTo(e - s, newElbow - s);
pose.SetWorldRotation(arm.UpperArm, MathQ.Normalize(upperDelta * upper.Rot));
// Then the forearm so the wrist lands on the (clamped) target.
world = pose.World;
var elbowNow = world[arm.LowerArm].Pos;
var wristNow = world[arm.Hand].Pos;
var clampedTarget = s + d * c;
var lowerDelta = MathQ.FromTo(wristNow - elbowNow, clampedTarget - elbowNow);
pose.SetWorldRotation(arm.LowerArm, MathQ.Normalize(lowerDelta * world[arm.LowerArm].Rot));
// Hand orientation.
pose.SetWorldRotation(arm.Hand, desired.Rot);
var bendDeg = 180f - MathF.Acos(Math.Clamp((a * a + b * b - c * c) / (2f * a * b), -1f, 1f)) * 180f / MathF.PI;
return new ReachResult(shortfall < 0.05f, shortfall, bendDeg);
}
/// <summary>Applies finger joint rotations of a hand pose to a character pose.</summary>
public static void ApplyFingers(CharacterPose pose, HandRig rig, HandPose hand, float weight = 1f)
{
foreach (var finger in rig.Fingers)
for (var j = 0; j < finger.Joints.Length; j++)
{
var bone = finger.Joints[j];
var target = hand.LocalRotation(rig, finger, j);
pose.Locals[bone].Rot = weight >= 0.9999f ? target : MathQ.Slerp(pose.Locals[bone].Rot, target, weight);
}
pose.Invalidate();
}
}