Editor-side planner for the support hand during reloads. Samples the character's animated wrist in weapon space, picks the moments where the hand is near the weapon or magazine, and builds contact keys (release, lock onto magazine, lock back on grip) with blend times scaled to the correction distance. Also has a no-magazine variant and offset helpers.
#nullable enable annotations
using System.Numerics;
using WeaponImporter.EditorTools.Core.Hands;
using WeaponImporter.EditorTools.Core.Maths;
namespace WeaponImporter.EditorTools.Core.Setup;
using Vector3 = System.Numerics.Vector3;
/// <summary>
/// Plans when the support hand holds what during an action, from the character's own animation
/// sampled over the action (the hand's wrist in weapon space). The source motion is kept: the
/// hand only locks onto a target while its animation already brings it close, so each
/// correction is as small as the animation allows, and it plays its own motion in between.
/// </summary>
public static class ContactPlanner
{
/// <summary>The animated support wrist at a normalized action time, in weapon space.</summary>
public readonly record struct Sample(float Time, XForm Wrist);
/// <summary>How close to the weapon (its bounds, or the magazine without them) the wrist must come for a touch, inches.</summary>
public const float TouchRadius = 8f;
/// <summary>How much worse than a touch's best moment a neighbouring moment may be and still belong to it.</summary>
public const float TouchBand = 2.5f;
/// <summary>Inches of touch cost per inch/second of hand speed (a working hand slows down).</summary>
public const float SpeedWeight = 0.05f;
/// <summary>Inches of touch cost per inch of distance to this weapon's magazine.</summary>
public const float MagazineWeight = 0.15f;
/// <summary>The hand counts as having left the grip once its animation moved this far (inches).</summary>
public const float LeaveDistance = 2.5f;
/// <summary>After its farthest point the hand is coming home once it is this close to its final place.</summary>
public const float HomeRadius = 6f;
/// <summary>A second touch is kept only when its correction is at most this large (inches)...</summary>
public const float SecondTouchMaxShift = 10f;
/// <summary>...and not much larger than the main touch's (inches).</summary>
public const float SecondTouchSlack = 4f;
/// <summary>Speed (in/s) a corrected hand may be pulled toward its target; sets the blend length.</summary>
public const float CorrectionSpeed = 30f;
/// <summary>Blend length for a correction of <paramref name="shift"/> inches.</summary>
public static float BlendFor(float shift) => Math.Clamp(0.12f + shift / CorrectionSpeed, TouchBlend, 0.6f);
public const float ReleaseBlend = 0.18f;
public const float TouchBlend = 0.16f;
public const float ReturnBlend = 0.28f;
public sealed record Plan(List<ContactKey> Keys, List<(float Start, float End)> Touches, float ClosestApproach, string Note);
/// <summary>
/// Reload plan: grip → release as the animation leaves → lock on the magazine while the
/// animated hand is at it (up to two touches: remove, insert) → back on the grip when the
/// animation returns. Without usable samples the hand simply lets go and comes back.
/// </summary>
public static Plan PlanReload(IReadOnlyList<Sample> samples, XForm grip, XForm magazine, float seconds, Geometry.Bounds? weapon = null, float fallbackRelease = 0.18f, float fallbackReturn = 0.86f)
{
seconds = MathF.Max(0.2f, seconds);
float N(float s) => s / seconds; // seconds -> normalized
var offset = OffsetFrom(grip, magazine);
if (samples.Count < 4)
return Fallback("no samples of the character's reload", float.NaN);
// Where the animation works the magazine: the hand has left its hold, is close to the
// weapon and slows down (grabbing, pulling, inserting). The character's animation was made
// for its own weapon, so these moments are found on the weapon as a whole and then
// retargeted onto this weapon's magazine. Near-magazine moments are preferred.
var dt = seconds / MathF.Max(1, samples.Count - 1);
var d = new float[samples.Count];
// The farthest point of the hand's trip splits leaving from coming home.
var far = Enumerable.Range(0, samples.Count).MaxBy(i => Vector3.Distance(samples[i].Wrist.Pos, samples[0].Wrist.Pos));
for (var i = 0; i < samples.Count; i++)
{
var p = samples[i].Wrist.Pos;
var away = i <= far
? Vector3.Distance(p, samples[0].Wrist.Pos) >= LeaveDistance
: Vector3.Distance(p, samples[^1].Wrist.Pos) >= HomeRadius;
var toMag = Vector3.Distance(p, magazine.Pos);
var toWeapon = weapon is { } b ? MathF.Sqrt(b.DistanceSquared(p)) : toMag;
var speed = Vector3.Distance(samples[Math.Min(i + 1, samples.Count - 1)].Wrist.Pos, samples[Math.Max(i - 1, 0)].Wrist.Pos) / (2f * dt);
d[i] = away && toWeapon <= TouchRadius ? toWeapon + speed * SpeedWeight + toMag * MagazineWeight : float.MaxValue;
}
var closest = d.Min();
if (closest == float.MaxValue)
return Fallback("the reload never brings the hand to the weapon", float.NaN);
// Touch windows: around each local minimum below the radius, while within the band.
var touches = new List<(int A, int B)>();
var used = new bool[samples.Count];
foreach (var i in Enumerable.Range(0, samples.Count).OrderBy(i => d[i]))
{
if (used[i] || d[i] == float.MaxValue || touches.Count >= 2)
continue;
var limit = d[i] + TouchBand;
int a = i, b = i;
while (a > 0 && !used[a - 1] && d[a - 1] <= limit) a--;
while (b < samples.Count - 1 && !used[b + 1] && d[b + 1] <= limit) b++;
for (var k = a; k <= b; k++) used[k] = true;
// Keep the windows apart so the hand is released in between.
for (var k = Math.Max(0, a - 2); k <= Math.Min(samples.Count - 1, b + 2); k++) used[k] = true;
touches.Add((a, b));
}
// How far each touch moves the hand off its animation (the correction it needs).
float Shift((int A, int B) w) => Enumerable.Range(w.A, w.B - w.A + 1).Min(i => Vector3.Distance(samples[i].Wrist.Pos, magazine.Pos));
// The smallest correction is the touch that matters; another one only when it asks for a
// similar, modest move (a big second pull reads as the arm being yanked around).
var primary = touches.OrderBy(Shift).First();
var primaryShift = Shift(primary);
touches = touches.Where(w => w == primary || Shift(w) <= MathF.Min(SecondTouchMaxShift, primaryShift + SecondTouchSlack)).ToList();
touches.Sort((x, y) => x.A.CompareTo(y.A));
var start = samples[0].Wrist.Pos;
var end = samples[^1].Wrist.Pos;
var firstTouch = samples[touches[0].A].Time;
var lastTouch = samples[touches[^1].B].Time;
// Leaves the grip: first moment the animation moved away from where it started.
var leave = samples.FirstOrDefault(s => s.Time < firstTouch && Vector3.Distance(s.Wrist.Pos, start) > LeaveDistance).Time;
if (leave <= 0f)
leave = MathF.Max(0.02f, firstTouch - N(ReleaseBlend + TouchBlend) - 0.05f);
// Back at the grip: first moment after the last touch the animation is home again.
var back = samples.FirstOrDefault(s => s.Time > lastTouch && Vector3.Distance(s.Wrist.Pos, end) < LeaveDistance).Time;
if (back <= 0f)
back = MathF.Min(0.95f, MathF.Max(lastTouch + 0.05f, fallbackReturn));
var keys = new List<ContactKey>();
var cursor = 0f;
void Add(float t, ContactState state, float blend, float[]? off, string target)
{
t = Math.Clamp(MathF.Max(t, cursor), 0f, 0.98f);
keys.Add(new ContactKey { Hand = Side.Left, Time = t, State = state, Blend = blend, Offset = off, Target = target });
// The next key starts only once this blend is over (a lock never starts from a half release).
cursor = t + N(blend) + 0.005f;
}
Add(leave - N(ReleaseBlend) * 0.5f, ContactState.Released, ReleaseBlend, null, "");
var touchList = new List<(float, float)>();
foreach (var window in touches)
{
var ta = samples[window.A].Time;
var tb = samples[window.B].Time;
// The farther the hand is taken off its animation, the longer it eases in and out,
// so it never moves faster than a hand would.
var blend = BlendFor(Shift(window));
Add(ta - N(blend), ContactState.Locked, blend, offset, "magazine");
var lockedAt = keys[^1].Time + N(blend);
Add(MathF.Max(tb, lockedAt), ContactState.Released, blend, null, "");
touchList.Add((lockedAt, keys[^1].Time));
}
Add(back - N(ReturnBlend), ContactState.Locked, ReturnBlend, null, "grip");
// How far the hand is moved onto this weapon's magazine at each touch (the correction size).
var shift = touches.Max(t => Enumerable.Range(t.A, t.B - t.A + 1).Min(i => Vector3.Distance(samples[i].Wrist.Pos, magazine.Pos)));
return new Plan(keys, touchList, shift, $"{touches.Count} magazine touch{(touches.Count == 1 ? "" : "es")}, hand moved up to {shift:0.0} in onto this magazine");
Plan Fallback(string why, float c)
{
var k = new List<ContactKey>
{
new() { Hand = Side.Left, Time = fallbackRelease, State = ContactState.Released, Blend = 0.3f },
new() { Hand = Side.Left, Time = fallbackReturn, State = ContactState.Locked, Blend = 0.3f, Target = "grip" },
};
return new Plan(k, new List<(float, float)>(), c, why);
}
}
/// <summary>
/// Reload without magazine contact: the support hand lets go when the character's animation
/// takes it off the weapon and comes back when the animation returns it, so the reload plays
/// exactly as animated in between. Timing comes from the animation, not fixed fractions.
/// </summary>
public static Plan PlanRelease(IReadOnlyList<Sample> samples, float seconds, float fallbackRelease = 0.18f, float fallbackReturn = 0.86f)
{
seconds = MathF.Max(0.2f, seconds);
float N(float s) => s / seconds;
var release = fallbackRelease;
var back = fallbackReturn;
if (samples.Count >= 4)
{
var start = samples[0].Wrist.Pos;
var end = samples[^1].Wrist.Pos;
var far = Enumerable.Range(0, samples.Count).MaxBy(i => Vector3.Distance(samples[i].Wrist.Pos, start));
if (Vector3.Distance(samples[far].Wrist.Pos, start) > LeaveDistance * 2f)
{
var leave = samples.FirstOrDefault(s => Vector3.Distance(s.Wrist.Pos, start) > LeaveDistance);
release = MathF.Max(0f, leave.Time - N(ReleaseBlend) * 0.5f);
var home = samples.Skip(far).FirstOrDefault(s => Vector3.Distance(s.Wrist.Pos, end) < LeaveDistance);
back = home.Time > 0f ? MathF.Max(release + N(ReleaseBlend) + 0.01f, home.Time - N(ReturnBlend)) : fallbackReturn;
}
}
var keys = new List<ContactKey>
{
new() { Hand = Side.Left, Time = release, State = ContactState.Released, Blend = ReleaseBlend },
new() { Hand = Side.Left, Time = Math.Clamp(back, 0f, 0.98f), State = ContactState.Locked, Blend = ReturnBlend, Target = "grip" },
};
return new Plan(keys, new List<(float, float)>(), 0f, "the hand follows the character's reload");
}
/// <summary>
/// The contact key offset that turns the grip target into <paramref name="target"/>
/// (position added, rotation pre-multiplied, as WeaponHold applies it).
/// </summary>
public static float[] OffsetFrom(XForm grip, XForm target)
{
var rot = MathQ.Normalize(target.Rot * Quaternion.Conjugate(grip.Rot));
var pos = target.Pos - grip.Pos;
return new[] { pos.X, pos.Y, pos.Z, rot.X, rot.Y, rot.Z, rot.W };
}
/// <summary>Applies an offset like the runtime does (for checks and previews).</summary>
public static XForm ApplyOffset(XForm grip, float[]? offset)
{
if (offset is not { Length: 7 })
return grip;
var rot = MathQ.Normalize(new Quaternion(offset[3], offset[4], offset[5], offset[6]));
return new XForm(grip.Pos + new Vector3(offset[0], offset[1], offset[2]), MathQ.Normalize(rot * grip.Rot));
}
}