🔍 s&box Package Code Search
Search C# source code, UI razor templates, shaders, and configs across s&box packages.
🔗 Raw Facepunch API Link: https://public.facepunch.com/sbox/code/search/1/?ident=notpointless.chomnr_better_ik&take=20
Showing code results for query:
*
(75 total matches found)
Game
library
#nullable enable
using System.Diagnostics.CodeAnalysis;
using System.Linq;
using Sandbox;
using BetterIk.Maths;
using BetterIk.Skeleton;
namespace BetterIk;
/// <summary>
/// Unconstrained FABRIK for variable-length chains (tails, tentacles, ropes). No pole vector, no
/// joint limits, no per-joint stiffness - for hinge-like limb IK, use TwoBoneIK instead. Drop on
/// the model root (or any descendant), set RootBone and EndBone to the chain's two ends; unlike
/// TwoBoneIK this cannot auto-walk a fixed depth, since a FABRIK chain's length is arbitrary, so
/// RootBone must be named explicitly.
/// </summary>
public sealed class FabrikIK : Component, IHasSkinnedRenderer
{
[Property] public SkinnedModelRenderer? Renderer { get; set; }
[Property, BoneName] public string RootBone { get; set; } = "";
[Property, BoneName] public string EndBone { get; set; } = "";
[Property] public GameObject? Target { get; set; }
[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;
[Property, Group( "Advanced" )] public int MaxIterations { get; set; } = 16;
// 0 = auto: 0.001 * total chain length, computed at solve time from the animated pose.
[Property, Group( "Advanced" )] public float Tolerance { get; set; } = 0f;
public bool HasValidChain { get; private set; }
public bool LastSolveApplied { get; private set; }
private (SkinnedModelRenderer Renderer, string RootBone, string EndBone) _cachedSignature;
// Root-first, end-last. Only valid once EnsureResolved() has returned true at least once.
private BoneCollection.Bone[] _chainBones = null!;
private Vector3[] _chainScales = null!;
protected override void OnStart()
{
Renderer ??= GameObject.GetComponent<SkinnedModelRenderer>()
?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );
}
protected override void OnPreRender()
{
Solve();
}
private void Solve()
{
if ( !EnsureResolved() )
{
HasValidChain = false;
LastSolveApplied = false;
return;
}
HasValidChain = true;
if ( Target is null || !Target.IsValid )
{
LastSolveApplied = false;
return;
}
// Weight <= 0 skips the read-solve-write cycle entirely rather than reading the animated
// pose and writing the identical value back. Live testing found the latter is NOT
// perfectly lossless through the world<->model-local round trip (Renderer.ToModelLocal)
// every frame: on Rig B's tail, repeatedly reading TryGetBoneTransformAnimation then
// writing the visually-unchanged result back via SetBoneTransform compounded into
// Infinity within a few seconds, even with bone scale frozen (Renderer.ToModelLocal
// finding above). Not writing at all when there is nothing to blend is strictly safer.
if ( Weight <= 0f )
{
LastSolveApplied = false;
return;
}
int n = _chainBones.Length;
var positions = new System.Numerics.Vector3[n];
var rotations = new System.Numerics.Quaternion[n];
float totalLength = 0f;
for ( int i = 0; i < n; i++ )
{
Renderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );
positions[i] = tx.Position.ToNumerics();
rotations[i] = tx.Rotation.ToNumerics();
if ( i > 0 )
totalLength += (positions[i] - positions[i - 1]).Length();
}
float tolerance = Tolerance > 0f ? Tolerance : 0.001f * MathF.Max( totalLength, 1e-4f );
var input = new FabrikInput
{
JointPositions = positions,
TargetPosition = Target.WorldPosition.ToNumerics(),
Weight = Weight,
MaxIterations = MaxIterations,
Tolerance = tolerance,
};
var result = FabrikSolver.Solve( input );
var solvedRotations = FabrikSolver.DeriveRotations( positions, result.JointPositions, rotations );
for ( int i = 0; i < n; i++ )
{
Renderer.SetBoneTransform( in _chainBones[i], Renderer.ToModelLocal(
new global::Transform( result.JointPositions[i].ToSandbox(), solvedRotations[i].ToSandbox() ).WithScale( _chainScales[i] ) ) );
}
// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML
// docs; kept defensively, matching the other components' already-verified usage.
#pragma warning disable CS0612
Renderer.PostAnimationUpdate();
#pragma warning restore CS0612
LastSolveApplied = true;
}
[MemberNotNullWhen( true, nameof( Renderer ) )]
private bool EnsureResolved()
{
if ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( RootBone ) || string.IsNullOrEmpty( EndBone ) )
return false;
var signature = (Renderer, RootBone, EndBone);
if ( signature.Equals( _cachedSignature ) && _chainBones is not null )
return true;
var bones = Renderer.Model.Bones;
if ( !bones.HasBone( EndBone ) )
return false;
IBoneNode endNode = new SandboxBoneNode( bones.GetBone( EndBone ) );
var chain = BoneChainResolver.ResolveNamedChain( endNode, RootBone );
if ( !chain.Success )
return false;
_chainBones = chain.Chain!.Select( node => ((SandboxBoneNode)node).Bone ).ToArray();
_cachedSignature = signature;
// Cached once here rather than re-read from TryGetBoneTransformAnimation every frame.
// Re-reading and re-writing world scale every frame round-trips it through
// Renderer.ToModelLocal on every solve; live testing found this compounds into Infinity
// within a few seconds on Rig B's tail (bone scale is not exactly 1 on that rig). IK never
// needs to change a bone's scale, so freezing it at first resolution is both safe and
// removes the feedback path entirely.
_chainScales = new Vector3[_chainBones.Length];
for ( int i = 0; i < _chainBones.Length; i++ )
{
Renderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );
_chainScales[i] = tx.Scale;
}
return true;
}
protected override void DrawGizmos()
{
if ( !EnsureResolved() )
{
Gizmo.Draw.Color = Color.Red;
Gizmo.Draw.WorldText( "FabrikIK: invalid chain", new global::Transform( WorldPosition ) );
return;
}
if ( Target is null || !Target.IsValid )
return;
Gizmo.Draw.Color = Color.Yellow;
for ( int i = 0; i < _chainBones.Length - 1; i++ )
{
Renderer.TryGetBoneTransform( in _chainBones[i], out var a );
Renderer.TryGetBoneTransform( in _chainBones[i + 1], out var b );
Gizmo.Draw.Line( a.Position, b.Position );
}
Gizmo.Draw.Color = Color.White;
Gizmo.Draw.LineSphere( new Sphere( Target.WorldPosition, 2f ) );
}
}
Game
library
namespace BetterIk.Maths;
/// <summary>Output of <see cref="FabrikSolver.Solve"/>.</summary>
public struct FabrikResult
{
/// <summary>Solved joint positions, world space, same length and root-first ordering as the input.</summary>
public System.Numerics.Vector3[] JointPositions;
public int IterationsUsed;
/// <summary>True only if the end joint landed within Tolerance of the target. False for the
/// unreachable-target clamp, the target-at-root passthrough, and any case that used every
/// available iteration without reaching tolerance - not an error signal, just "did not land
/// exactly on target", matching the geometric reality of a rigid chain.</summary>
public bool Converged;
}
Game
library
#nullable enable
namespace BetterIk.Skeleton;
/// <summary>Minimal shape needed to walk a skeleton toward the root. Implemented by a real
/// BoneCollection.Bone adapter in the engine layer, and by a fake node in tests.</summary>
public interface IBoneNode
{
string Name { get; }
/// <summary>Null at the skeleton root.</summary>
IBoneNode? Parent { get; }
}
Game
library
#nullable enable
using Sandbox;
namespace BetterIk;
/// <summary>Implemented by every IK component that operates on a SkinnedModelRenderer, so editor
/// tooling (BoneNameControlWidget) can resolve the renderer to list bones without reflection.</summary>
public interface IHasSkinnedRenderer
{
SkinnedModelRenderer? Renderer { get; }
}
Game
library
namespace BetterIk.Maths;
using Vector3 = System.Numerics.Vector3;
/// <summary>Precomputed once per bone from the bind pose. Result of <see cref="LookAtSolver.AnalyzeBindPose"/>.</summary>
public readonly struct LookAtBindData
{
/// <summary>Unit, bone-local space direction the bone should be treated as "facing".</summary>
public readonly Vector3 LocalAimDirection;
/// <summary>False if no usable child bone existed in the bind pose (leaf bone, or a degenerate
/// child offset); <see cref="LocalAimDirection"/> is still a valid unit vector, just an arbitrary
/// deterministic fallback rather than an anatomically meaningful one.</summary>
public readonly bool IsReliable;
public LookAtBindData(Vector3 localAimDirection, bool isReliable)
{
LocalAimDirection = localAimDirection;
IsReliable = isReliable;
}
}
Game
library
global using static Sandbox.Internal.GlobalGameNamespace;
global using Microsoft.AspNetCore.Components;
global using Microsoft.AspNetCore.Components.Rendering;
[assembly: global::System.Reflection.AssemblyMetadata( "AddonTitle", "Better IK" )]
[assembly: global::System.Reflection.AssemblyMetadata( "AddonIdent", "chomnr_better_ik" )]
[assembly: global::System.Reflection.AssemblyMetadata( "OrgIdent", "notpointless" )]
[assembly: global::System.Reflection.AssemblyMetadata( "Ident", "notpointless.chomnr_better_ik" )]
[assembly: global::System.Reflection.AssemblyMetadata( "EngineVersion", "28" )]
[assembly: global::System.Reflection.AssemblyMetadata( "EngineMinorVersion", "1" )]
[assembly: System.Runtime.Versioning.TargetFramework( ".NETCoreApp,Version=v9.0", FrameworkDisplayName = ".NET 9.0" )]
[assembly: global::System.Reflection.AssemblyMetadata( "CompileTime", "2026-07-18T00:05:12.5911669Z" )]
[assembly: global::System.Reflection.AssemblyVersion("0.0.119.0")]
[assembly: global::System.Reflection.AssemblyFileVersion("0.0.119.0")]
Game
library
namespace BetterIk.Maths;
/// <summary>
/// Trailing-minimum tracker over a fixed time window. Feed it one plant-point height per
/// frame with the current time; <see cref="Push"/> returns the lowest value seen within the
/// last <see cref="WindowSeconds"/>. Plant-to-ground foot placement uses it to find the
/// "planted" level of a cyclic animation (the lowest the foot reached recently), so that only
/// that constant is removed and authored lifts above it are preserved.
///
/// This is the one stateful helper in the Maths layer; the solvers themselves stay pure. It is
/// engine-agnostic (floats and a time value only), so it is unit-testable without an editor,
/// like the rest of the core. Samples are assumed pushed in non-decreasing time order (the
/// engine wall-clock during a frame loop); the newest <see cref="Capacity"/> samples are
/// retained, so at very high frame rates a window longer than the buffer can hold is measured
/// over as many recent samples as fit rather than the full duration.
/// </summary>
public sealed class PlantWindow
{
private readonly float[] _time;
private readonly float[] _value;
private int _head; // index of the next write
private int _count;
/// <summary>Length of the trailing window in the same time unit passed to <see cref="Push"/>.</summary>
public float WindowSeconds { get; set; }
public int Capacity => _time.Length;
public bool HasSamples => _count > 0;
public PlantWindow(float windowSeconds, int capacity = 512)
{
if (capacity < 1)
capacity = 1;
WindowSeconds = windowSeconds;
_time = new float[capacity];
_value = new float[capacity];
}
/// <summary>Discard all samples (e.g. on a hard clip cut) so the window rebuilds from scratch.</summary>
public void Reset()
{
_head = 0;
_count = 0;
}
/// <summary>
/// Record a sample and return the trailing minimum over the window ending at
/// <paramref name="now"/>. With an empty history this is just <paramref name="value"/>.
/// </summary>
public float Push(float now, float value)
{
_time[_head] = now;
_value[_head] = value;
_head = (_head + 1) % _time.Length;
if (_count < _time.Length)
_count++;
return MinSince(now);
}
/// <summary>Trailing minimum over the window ending at <paramref name="now"/> without adding a sample.</summary>
public float MinSince(float now)
{
if (_count == 0)
return 0f;
float window = WindowSeconds;
if (window < 0f)
window = 0f;
float cutoff = now - window;
float min = float.MaxValue;
// Walk from the most recent sample backward. Time is non-decreasing in push order, so
// the first sample older than the cutoff means every earlier one is out of window too.
for (int i = 0; i < _count; i++)
{
int idx = _head - 1 - i;
if (idx < 0)
idx += _time.Length;
if (_time[idx] < cutoff)
break;
if (_value[idx] < min)
min = _value[idx];
}
// At least the newest sample is always within [cutoff, now] when window >= 0.
return min == float.MaxValue ? _value[(_head - 1 + _time.Length) % _time.Length] : min;
}
}
Game
library
#nullable enable
using System.Diagnostics.CodeAnalysis;
using Sandbox;
using BetterIk.Maths;
using BetterIk.Skeleton;
namespace BetterIk;
/// <summary>
/// Runtime two-bone IK (arm/leg) with pole vector control. Drop on the model root (or any
/// descendant of it - the component finds the SkinnedModelRenderer via GetComponentInParent),
/// set EndBone to the hand/foot bone name, optionally drag in a Target and PoleTarget. Walks
/// up the skeleton automatically to find the mid and root bones; no per-model setup needed.
/// </summary>
public sealed class TwoBoneIK : Component, IHasSkinnedRenderer
{
// --- Setup (the happy path: add component, set EndBone, drag Target, done) ---
[Property] public SkinnedModelRenderer? Renderer { get; set; }
[Property, BoneName] public string EndBone { get; set; } = "";
[Property] public GameObject? Target { get; set; }
[Property] public GameObject? PoleTarget { get; set; }
// --- Weights (all lerp-safe every frame) ---
[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;
[Property, Range( 0f, 1f )] public float PositionWeight { get; set; } = 1f;
[Property, Range( 0f, 1f )] public float RotationWeight { get; set; } = 1f;
// --- Advanced: manual bone overrides, pole fine-tuning, soft/stretch (off by default) ---
[Property, BoneName, Group( "Advanced" )] public string RootBoneOverride { get; set; } = "";
[Property, BoneName, Group( "Advanced" )] public string MidBoneOverride { get; set; } = "";
[Property, Group( "Advanced" ), Range( -180f, 180f )] public float PoleAngleOffsetDegrees { get; set; } = 0f;
[Property, Group( "Advanced" ), Range( 0f, 0.49f )] public float SoftFraction { get; set; } = 0f;
[Property, Group( "Advanced" ), Range( 0f, 1f )] public float MaxStretch { get; set; } = 0f;
// --- Diagnostics ---
public bool HasValidChain { get; private set; }
public bool LastSolveApplied { get; private set; }
private (SkinnedModelRenderer Renderer, string EndBone, string RootOverride, string MidOverride) _cachedSignature;
// Only valid once EnsureResolved() has returned true at least once, same as a RequireComponent
// field is only valid once OnStart has run.
private BoneCollection.Bone _rootBone = null!;
private BoneCollection.Bone _midBone = null!;
private BoneCollection.Bone _endBone = null!;
private BindPoseData _bindPose;
private Vector3 _lastPoleDirection = Vector3.Up;
protected override void OnStart()
{
Renderer ??= GameObject.GetComponent<SkinnedModelRenderer>()
?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );
}
protected override void OnPreRender()
{
Solve();
}
private void Solve()
{
if ( !EnsureResolved() )
{
HasValidChain = false;
LastSolveApplied = false;
return;
}
HasValidChain = true;
if ( Target is null || !Target.IsValid )
{
LastSolveApplied = false;
return;
}
// Weight <= 0 skips the read-solve-write cycle entirely rather than reading the animated
// pose and writing an "unchanged" result back. The world<->model-local round trip through
// SetBoneTransform is NOT perfectly lossless every frame - on a rig with no real animation
// driving it, this compounded into Infinity within a few seconds even though the math is
// a no-op at Weight=0. Not writing at all when there is nothing to blend removes the
// feedback path entirely.
if ( Weight <= 0f )
{
LastSolveApplied = false;
return;
}
Renderer.TryGetBoneTransformAnimation( in _rootBone, out var rootTx );
Renderer.TryGetBoneTransformAnimation( in _midBone, out var midTx );
Renderer.TryGetBoneTransformAnimation( in _endBone, out var endTx );
Vector3 poleHint = PoleTarget is not null && PoleTarget.IsValid
? PoleTarget.WorldPosition - rootTx.Position
: rootTx.Rotation * _bindPose.DefaultPoleDirection.ToSandbox();
_lastPoleDirection = poleHint.Normal;
var input = new TwoBoneIkInput
{
RootPosition = rootTx.Position.ToNumerics(),
MidPosition = midTx.Position.ToNumerics(),
EndPosition = endTx.Position.ToNumerics(),
RootRotation = rootTx.Rotation.ToNumerics(),
MidRotation = midTx.Rotation.ToNumerics(),
EndRotation = endTx.Rotation.ToNumerics(),
TargetPosition = Target.WorldPosition.ToNumerics(),
TargetRotation = Target.WorldRotation.ToNumerics(),
HasPole = true, // poleHint always carries either the real pole or the bind-pose default
PoleHint = poleHint.ToNumerics(),
PoleAngleOffsetRadians = PoleAngleOffsetDegrees * (MathF.PI / 180f),
FallbackBendNormal = _bindPose.BendNormal,
PositionWeight = PositionWeight,
RotationWeight = RotationWeight,
MasterWeight = Weight,
SoftFraction = SoftFraction,
MaxStretch = MaxStretch,
};
var result = TwoBoneIkSolver.Solve( input );
// SetBoneTransform expects model-local space, unlike TryGetBoneTransformAnimation/
// TryGetBoneTransform which are documented as worldspace - see MathBridge.ToModelLocal.
Renderer.SetBoneTransform( in _rootBone, Renderer.ToModelLocal( new global::Transform( rootTx.Position, result.RootRotation.ToSandbox() ).WithScale( rootTx.Scale ) ) );
Renderer.SetBoneTransform( in _midBone, Renderer.ToModelLocal( new global::Transform( result.MidPosition.ToSandbox(), result.MidRotation.ToSandbox() ).WithScale( midTx.Scale ) ) );
Renderer.SetBoneTransform( in _endBone, Renderer.ToModelLocal( new global::Transform( result.EndPosition.ToSandbox(), result.EndRotation.ToSandbox() ).WithScale( endTx.Scale ) ) );
// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML
// docs; kept defensively until checklist item 3 is verified against a live editor.
#pragma warning disable CS0612
Renderer.PostAnimationUpdate();
#pragma warning restore CS0612
LastSolveApplied = result.Solved;
}
[MemberNotNullWhen( true, nameof( Renderer ) )]
private bool EnsureResolved()
{
if ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( EndBone ) )
return false;
var signature = (Renderer, EndBone, RootBoneOverride, MidBoneOverride);
if ( signature.Equals( _cachedSignature ) && _rootBone is not null )
return true;
var bones = Renderer.Model.Bones;
if ( !bones.HasBone( EndBone ) )
return false;
IBoneNode endNode = new SandboxBoneNode( bones.GetBone( EndBone ) );
IBoneNode? rootOverrideNode = null;
if ( !string.IsNullOrEmpty( RootBoneOverride ) )
{
if ( !bones.HasBone( RootBoneOverride ) )
return false;
rootOverrideNode = new SandboxBoneNode( bones.GetBone( RootBoneOverride ) );
}
IBoneNode? midOverrideNode = null;
if ( !string.IsNullOrEmpty( MidBoneOverride ) )
{
if ( !bones.HasBone( MidBoneOverride ) )
return false;
midOverrideNode = new SandboxBoneNode( bones.GetBone( MidBoneOverride ) );
}
var chain = BoneChainResolver.Resolve( endNode, rootOverrideNode, midOverrideNode );
if ( !chain.Success )
return false;
_rootBone = ((SandboxBoneNode)chain.Root!).Bone;
_midBone = ((SandboxBoneNode)chain.Mid!).Bone;
_endBone = ((SandboxBoneNode)chain.End!).Bone;
_cachedSignature = signature;
// Bind-pose world positions, composed from an arbitrary origin at the root - only the
// relative offsets matter to AnalyzeBindPose (translation-invariant), so we don't need
// to walk all the way up to the model's true origin.
var rootBindWorld = global::Transform.Zero;
var midBindWorld = global::Transform.Concat( rootBindWorld, _midBone.LocalTransform );
var endBindWorld = global::Transform.Concat( midBindWorld, _endBone.LocalTransform );
_bindPose = TwoBoneIkSolver.AnalyzeBindPose(
rootBindWorld.Position.ToNumerics(),
midBindWorld.Position.ToNumerics(),
endBindWorld.Position.ToNumerics() );
return true;
}
protected override void DrawGizmos()
{
if ( !EnsureResolved() )
{
Gizmo.Draw.Color = Color.Red;
Gizmo.Draw.WorldText( "TwoBoneIK: invalid bone chain", new global::Transform( WorldPosition ) );
return;
}
if ( Target is null || !Target.IsValid )
return;
Renderer.TryGetBoneTransformAnimation( in _rootBone, out var rootTx );
Renderer.TryGetBoneTransformAnimation( in _midBone, out var midTx );
Renderer.TryGetBoneTransformAnimation( in _endBone, out var endTx );
float l1 = (midTx.Position - rootTx.Position).Length;
float l2 = (endTx.Position - midTx.Position).Length;
float lmax = l1 + l2;
float d = (Target.WorldPosition - rootTx.Position).Length;
float minReach = MathF.Abs( l1 - l2 );
float softStart = (1f - SoftFraction) * lmax;
Color reachColor;
if ( d < minReach || d > lmax * (1f + MaxStretch) )
reachColor = Color.Red;
else if ( SoftFraction > 0f && d > softStart )
reachColor = Color.Yellow;
else
reachColor = Color.Green;
// Solved chain (post-IK), reusing the same read this frame's Solve() already wrote.
Renderer.TryGetBoneTransform( in _rootBone, out var solvedRoot );
Renderer.TryGetBoneTransform( in _midBone, out var solvedMid );
Renderer.TryGetBoneTransform( in _endBone, out var solvedEnd );
Gizmo.Draw.Color = reachColor;
Gizmo.Draw.Line( solvedRoot.Position, solvedMid.Position );
Gizmo.Draw.Line( solvedMid.Position, solvedEnd.Position );
Gizmo.Draw.Color = Color.White;
Gizmo.Draw.LineSphere( new Sphere( Target.WorldPosition, MathF.Max( lmax * 0.05f, 1f ) ) );
Gizmo.Draw.Color = PoleTarget is not null && PoleTarget.IsValid ? Color.White : Color.Cyan;
Gizmo.Draw.Arrow( solvedMid.Position, solvedMid.Position + _lastPoleDirection * (lmax * 0.3f), lmax * 0.05f, lmax * 0.03f );
Gizmo.Draw.Color = Color.Cyan.WithAlpha( 0.15f );
Gizmo.Draw.SolidTriangle( new Triangle { A = solvedRoot.Position, B = solvedMid.Position, C = solvedEnd.Position } );
}
}
Game
library
namespace BetterIk.Maths;
using Vector3 = System.Numerics.Vector3;
using Quaternion = System.Numerics.Quaternion;
using Matrix4x4 = System.Numerics.Matrix4x4;
internal static class IkMath
{
public static Vector3 SafeNormalize(Vector3 v, Vector3 fallback)
{
float lenSq = v.LengthSquared();
if (lenSq < 1e-12f)
return fallback;
return v / MathF.Sqrt(lenSq);
}
public static Vector3 ProjectPerpendicular(Vector3 v, Vector3 unitAxis)
{
return v - Vector3.Dot(v, unitAxis) * unitAxis;
}
// Deterministic arbitrary perpendicular: cross with whichever world axis has the
// smallest component along unitAxis, so the result never collapses to zero.
public static Vector3 AnyPerpendicular(Vector3 unitAxis)
{
float ax = MathF.Abs(unitAxis.X);
float ay = MathF.Abs(unitAxis.Y);
float az = MathF.Abs(unitAxis.Z);
Vector3 seed = (ax <= ay && ax <= az) ? Vector3.UnitX
: (ay <= az) ? Vector3.UnitY
: Vector3.UnitZ;
Vector3 perp = Vector3.Cross(unitAxis, seed);
if (perp.LengthSquared() < 1e-12f)
{
seed = seed == Vector3.UnitX ? Vector3.UnitY : Vector3.UnitX;
perp = Vector3.Cross(unitAxis, seed);
}
return Vector3.Normalize(perp);
}
// U = dir, W = Gram-Schmidt of normalHint against U (fallback to AnyPerpendicular if degenerate), V = U x W.
public static (Vector3 U, Vector3 W, Vector3 V) BuildOrthonormalBasis(Vector3 dir, Vector3 normalHint)
{
Vector3 u = SafeNormalize(dir, Vector3.UnitX);
Vector3 wRaw = ProjectPerpendicular(normalHint, u);
Vector3 w = wRaw.LengthSquared() < 1e-10f ? AnyPerpendicular(u) : Vector3.Normalize(wRaw);
Vector3 v = Vector3.Cross(u, w);
return (u, w, v);
}
// Rotation mapping the old (dir, normalHint) frame exactly onto the new (dir, normalHint) frame:
// delta * oldU = newU, delta * oldW = newW, delta * oldV = newV. Built via change-of-basis matrices
// rather than shortest-arc, so twist/roll is pinned and there is no 180-degree flip ambiguity.
public static Quaternion DeltaRotation(Vector3 oldDir, Vector3 oldNormalHint, Vector3 newDir, Vector3 newNormalHint)
{
var (uOld, wOld, vOld) = BuildOrthonormalBasis(oldDir, oldNormalHint);
var (uNew, wNew, vNew) = BuildOrthonormalBasis(newDir, newNormalHint);
// Row-vector convention (matches System.Numerics Vector3.Transform(v, Matrix4x4)):
// row i of the matrix is where local axis i maps to.
var mOld = new Matrix4x4(
uOld.X, uOld.Y, uOld.Z, 0f,
wOld.X, wOld.Y, wOld.Z, 0f,
vOld.X, vOld.Y, vOld.Z, 0f,
0f, 0f, 0f, 1f);
var mNew = new Matrix4x4(
uNew.X, uNew.Y, uNew.Z, 0f,
wNew.X, wNew.Y, wNew.Z, 0f,
vNew.X, vNew.Y, vNew.Z, 0f,
0f, 0f, 0f, 1f);
// mOld is orthonormal, so its inverse is its transpose: delta = mOld^-1 * mNew.
var delta = Matrix4x4.Transpose(mOld) * mNew;
return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(delta));
}
// Shortest-arc rotation mapping unit vector `from` onto unit vector `to`. No twist/roll control
// around the resulting axis (there is no well-defined "roll" for a pure vector-to-vector map).
internal static Quaternion FromToRotation(Vector3 from, Vector3 to)
{
float cosAngle = Math.Clamp(Vector3.Dot(from, to), -1f, 1f);
if (cosAngle > 1f - 1e-7f)
return Quaternion.Identity;
if (cosAngle < -1f + 1e-7f)
{
Vector3 axis180 = AnyPerpendicular(from);
return Quaternion.CreateFromAxisAngle(axis180, MathF.PI);
}
Vector3 axis = Vector3.Normalize(Vector3.Cross(from, to));
float angle = MathF.Acos(cosAngle);
return Quaternion.CreateFromAxisAngle(axis, angle);
}
}
Game
library
#nullable enable
namespace BetterIk.Skeleton;
public readonly struct BoneChainResult
{
public readonly bool Success;
public readonly BoneChainError Error;
public readonly IBoneNode? Root;
public readonly IBoneNode? Mid;
public readonly IBoneNode? End;
private BoneChainResult(bool success, BoneChainError error, IBoneNode? root, IBoneNode? mid, IBoneNode? end)
{
Success = success;
Error = error;
Root = root;
Mid = mid;
End = end;
}
public static BoneChainResult Ok(IBoneNode root, IBoneNode mid, IBoneNode end)
=> new(true, BoneChainError.None, root, mid, end);
public static BoneChainResult Fail(BoneChainError error)
=> new(false, error, null, null, null);
}
Game
library
#nullable enable
using System;
using System.Diagnostics.CodeAnalysis;
using Sandbox;
using BetterIk.Maths;
using BetterIk.Skeleton;
namespace BetterIk;
/// <summary>
/// Foot grounding: traces down under each foot, plants feet on real geometry, drops the pelvis
/// when the ground is lower so the far foot can reach, and aligns foot rotation to the surface
/// normal. Drop on the model root (or any descendant), set LeftFootBone/RightFootBone to the
/// ankle/foot bone names. Root and mid bones for each leg are auto-walked like TwoBoneIK; the
/// pelvis is auto-derived as the nearest common ancestor of the two resolved leg roots.
/// </summary>
public sealed class FootPlacementIK : Component, IHasSkinnedRenderer
{
[Property] public SkinnedModelRenderer? Renderer { get; set; }
[Property, BoneName] public string LeftFootBone { get; set; } = "";
[Property, BoneName] public string RightFootBone { get; set; } = "";
[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;
[Property, Range( 0f, 1f )] public float FootRotationWeight { get; set; } = 1f;
[Property, Group( "Ground" )] public float MaxStepUp { get; set; } = 18f;
[Property, Group( "Ground" )] public float MaxStepDown { get; set; } = 18f;
[Property, Group( "Ground" )] public float MaxPelvisDrop { get; set; } = 16f;
[Property, Group( "Ground" )] public float MaxPelvisRaise { get; set; } = 0f;
[Property, Group( "Ground" ), Range( 0f, 90f )] public float MaxFootRotationDegrees { get; set; } = 30f;
[Property, Group( "Ground" ), Range( 0f, 90f )] public float MaxGroundSlopeDegrees { get; set; } = 60f;
[Property, Group( "Ground" )] public float FootHeightOffset { get; set; } = 0f;
[Property, Group( "Ground" )] public float SmoothingRate { get; set; } = 10f;
[Property, Group( "Ground" )] public string IgnoreTags { get; set; } = "player";
// ---- Plant-to-ground mode (opt-in) -----------------------------------------------------
// The default (terrain) mode preserves the animation's authored height above an assumed
// flat ground and only adapts to terrain deviation, which is correct for grounded
// animations. Plant mode instead corrects animations whose feet HOVER above the ground
// they were authored to touch (a per-clip constant conversion offset, possibly different
// per foot): each foot's plant point (an optional plant bone, e.g. the ball of the foot;
// the foot bone itself when unset) is measured against the ground, its TRAILING-MINIMUM
// height over PlantWindowSeconds is treated as the hover to remove, and the foot is
// lowered vertically by that amount with its AUTHORED ROTATION UNTOUCHED. Removing only
// the trailing minimum keeps every within-window articulation intact: authored step
// lifts and heel raises ride on top of the corrected plant level, flat-authored feet
// come out flat, and nothing is ever rotated onto the surface. Pelvis is not moved in
// plant mode (correct a shared full-body offset at the model root instead).
[Property, Group( "Plant" )] public bool PlantToGround { get; set; } = false;
[Property, BoneName, Group( "Plant" )] public string LeftPlantBone { get; set; } = "";
[Property, BoneName, Group( "Plant" )] public string RightPlantBone { get; set; } = "";
/// <summary>Rest height of the plant point above the ground when planted (its bind height).</summary>
[Property, Group( "Plant" )] public float PlantHeightOffset { get; set; } = 0f;
/// <summary>Trailing window (seconds) whose minimum plant-point height is removed as hover.</summary>
[Property, Group( "Plant" )] public float PlantWindowSeconds { get; set; } = 0.8f;
/// <summary>Upper clamp on the per-foot plant correction.</summary>
[Property, Group( "Plant" )] public float PlantMaxCorrection { get; set; } = 6f;
/// <summary>Skip tracing and use a flat world-space ground plane at FlatGroundHeight.</summary>
[Property, Group( "Plant" )] public bool UseFlatGround { get; set; } = false;
[Property, Group( "Plant" )] public float FlatGroundHeight { get; set; } = 0f;
/// <summary>Live plant corrections (world units, positive = foot lowered), for diagnostics.</summary>
public float CurrentPlantCorrectionL { get; private set; }
public float CurrentPlantCorrectionR { get; private set; }
/// <summary>Live plant-point heights above the ground before correction, for diagnostics.</summary>
public float LastPlantResidualL { get; private set; }
public float LastPlantResidualR { get; private set; }
[Property, BoneName, Group( "Advanced" )] public string PelvisBoneOverride { get; set; } = "";
[Property, Group( "Advanced" )] public GameObject? LeftPoleTarget { get; set; }
[Property, Group( "Advanced" )] public GameObject? RightPoleTarget { get; set; }
[Property, BoneName, Group( "Advanced" )] public string LeftRootOverride { get; set; } = "";
[Property, BoneName, Group( "Advanced" )] public string LeftMidOverride { get; set; } = "";
[Property, BoneName, Group( "Advanced" )] public string RightRootOverride { get; set; } = "";
[Property, BoneName, Group( "Advanced" )] public string RightMidOverride { get; set; } = "";
public bool HasValidChains { get; private set; }
public bool PelvisResolved { get; private set; }
public bool LeftGrounded { get; private set; }
public bool RightGrounded { get; private set; }
public float CurrentPelvisOffset { get; private set; }
private (SkinnedModelRenderer Renderer, string LeftFootBone, string RightFootBone, string LeftRootOverride, string LeftMidOverride, string RightRootOverride, string RightMidOverride, string PelvisBoneOverride, string LeftPlantBone, string RightPlantBone) _cachedSignature;
// Only valid once EnsureResolved() has returned true at least once.
private BoneCollection.Bone _leftRoot = null!;
private BoneCollection.Bone _leftMid = null!;
private BoneCollection.Bone _leftEnd = null!;
private BoneCollection.Bone _rightRoot = null!;
private BoneCollection.Bone _rightMid = null!;
private BoneCollection.Bone _rightEnd = null!;
private BoneCollection.Bone _pelvisBone = null!;
// Plant points (plant mode): the ball / toe bone whose ground contact is planted. Falls
// back to the leg's own end bone when the name is unset or not present on the model.
private BoneCollection.Bone _leftPlant = null!;
private BoneCollection.Bone _rightPlant = null!;
private bool _leftPlantResolved;
private bool _rightPlantResolved;
private BindPoseData _leftBindPose;
private BindPoseData _rightBindPose;
private float _smoothPelvis;
private float _smoothDeltaL;
private float _smoothDeltaR;
// Plant mode state: trailing-minimum windows over each plant point's height above ground,
// and the smoothed per-foot downward correction actually applied.
private PlantWindow _plantWindowL;
private PlantWindow _plantWindowR;
private float _smoothPlantL;
private float _smoothPlantR;
protected override void OnStart()
{
Renderer ??= GameObject.GetComponent<SkinnedModelRenderer>()
?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );
}
protected override void OnPreRender()
{
Solve();
}
private void Solve()
{
if ( !EnsureResolved() )
{
HasValidChains = false;
LeftGrounded = false;
RightGrounded = false;
return;
}
HasValidChains = true;
// Weight <= 0 skips the entire read-trace-solve-write cycle: a SetBoneTransform whose
// value has no restoring force (not animated, not converging toward a fixed target) is
// not bit-lossless at the engine write boundary and compounds to Infinity over enough
// frames (confirmed via FabrikIK). Zero the smoothed state so re-enabling Weight later
// doesn't replay stale offsets.
if ( Weight <= 0f )
{
_smoothPelvis = 0f;
_smoothDeltaL = 0f;
_smoothDeltaR = 0f;
_smoothPlantL = 0f;
_smoothPlantR = 0f;
_plantWindowL?.Reset();
_plantWindowR?.Reset();
LeftGrounded = false;
RightGrounded = false;
CurrentPelvisOffset = 0f;
CurrentPlantCorrectionL = 0f;
CurrentPlantCorrectionR = 0f;
return;
}
if ( PlantToGround )
{
SolvePlant();
return;
}
Renderer.TryGetBoneTransformAnimation( in _leftRoot, out var leftRootTx );
Renderer.TryGetBoneTransformAnimation( in _leftMid, out var leftMidTx );
Renderer.TryGetBoneTransformAnimation( in _leftEnd, out var leftEndTx );
Renderer.TryGetBoneTransformAnimation( in _rightRoot, out var rightRootTx );
Renderer.TryGetBoneTransformAnimation( in _rightMid, out var rightMidTx );
Renderer.TryGetBoneTransformAnimation( in _rightEnd, out var rightEndTx );
var pelvisTx = global::Transform.Zero;
if ( PelvisResolved )
Renderer.TryGetBoneTransformAnimation( in _pelvisBone, out pelvisTx );
Vector3 up = Vector3.Up;
Vector3 origin = Renderer.WorldPosition;
var leftTrace = TraceFoot( leftEndTx.Position, up, origin );
var rightTrace = TraceFoot( rightEndTx.Position, up, origin );
var input = new FootPlacementInput
{
LeftFoot = new FootInput
{
FootPosition = leftEndTx.Position.ToNumerics(),
FootRotation = leftEndTx.Rotation.ToNumerics(),
HasHit = leftTrace.Hit,
HitPoint = leftTrace.HitPosition.ToNumerics(),
HitNormal = leftTrace.Normal.ToNumerics(),
},
RightFoot = new FootInput
{
FootPosition = rightEndTx.Position.ToNumerics(),
FootRotation = rightEndTx.Rotation.ToNumerics(),
HasHit = rightTrace.Hit,
HitPoint = rightTrace.HitPosition.ToNumerics(),
HitNormal = rightTrace.Normal.ToNumerics(),
},
OriginPosition = origin.ToNumerics(),
UpAxis = up.ToNumerics(),
FootHeightOffset = FootHeightOffset,
MaxStepUp = MaxStepUp,
MaxStepDown = MaxStepDown,
MaxPelvisDrop = MaxPelvisDrop,
MaxPelvisRaise = MaxPelvisRaise,
MaxFootRotationRadians = MaxFootRotationDegrees * (MathF.PI / 180f),
MaxGroundSlopeRadians = MaxGroundSlopeDegrees * (MathF.PI / 180f),
Weight = Weight,
};
var result = FootPlacementSolver.Solve( input );
float dt = Time.Delta;
_smoothPelvis = FootPlacementSolver.SmoothOffset( _smoothPelvis, result.PelvisOffset, SmoothingRate, dt );
_smoothDeltaL = FootPlacementSolver.SmoothOffset( _smoothDeltaL, result.LeftFoot.VerticalDelta, SmoothingRate, dt );
_smoothDeltaR = FootPlacementSolver.SmoothOffset( _smoothDeltaR, result.RightFoot.VerticalDelta, SmoothingRate, dt );
// Snap to exact zero once within epsilon of a zero target: exponential decay (SmoothOffset)
// never reaches exact 0 on its own, which would otherwise keep the hazardous "write back
// an unchanged value forever" pattern alive permanently on flat ground / an ungrounded foot.
if ( result.PelvisOffset == 0f && MathF.Abs( _smoothPelvis ) < 1e-3f )
_smoothPelvis = 0f;
if ( result.LeftFoot.VerticalDelta == 0f && MathF.Abs( _smoothDeltaL ) < 1e-3f )
_smoothDeltaL = 0f;
if ( result.RightFoot.VerticalDelta == 0f && MathF.Abs( _smoothDeltaR ) < 1e-3f )
_smoothDeltaR = 0f;
LeftGrounded = result.LeftFoot.Grounded;
RightGrounded = result.RightFoot.Grounded;
CurrentPelvisOffset = _smoothPelvis;
// Skip the pelvis write entirely once its offset has snapped to exact zero - a zero-offset
// write targets exactly the animated pose anyway, so skipping it is not a behavior change,
// only a removal of an unforced no-op write (see the principle above).
if ( PelvisResolved && _smoothPelvis != 0f )
{
// SetBoneTransform expects model-local space - see MathBridge.ToModelLocal.
Renderer.SetBoneTransform( in _pelvisBone, Renderer.ToModelLocal( new global::Transform( pelvisTx.Position + up * _smoothPelvis, pelvisTx.Rotation ).WithScale( pelvisTx.Scale ) ) );
}
Vector3 pelvisShift = up * (PelvisResolved ? _smoothPelvis : 0f);
// Skip a leg's writes only when there is genuinely nothing driving them: ungrounded (no
// trace hit), its smoothed delta has snapped to zero, AND the pelvis isn't shifting it
// either. A nonzero pelvis shift still requires the leg solve even for an otherwise-settled
// ungrounded foot, since the leg still needs to follow the pelvis. Otherwise, an ungrounded
// foot's TargetPosition is derived from this frame's own read (endTx.Position + up*0), which
// has no restoring force once settled - the same unforced-write hazard as the pelvis case.
bool leftNeedsWrite = result.LeftFoot.Grounded || _smoothDeltaL != 0f || pelvisShift != Vector3.Zero;
bool rightNeedsWrite = result.RightFoot.Grounded || _smoothDeltaR != 0f || pelvisShift != Vector3.Zero;
if ( leftNeedsWrite )
SolveLeg( in _leftRoot, in _leftMid, in _leftEnd, leftRootTx, leftMidTx, leftEndTx, _leftBindPose, pelvisShift, _smoothDeltaL, result.LeftFoot.TargetRotation, LeftPoleTarget, up );
if ( rightNeedsWrite )
SolveLeg( in _rightRoot, in _rightMid, in _rightEnd, rightRootTx, rightMidTx, rightEndTx, _rightBindPose, pelvisShift, _smoothDeltaR, result.RightFoot.TargetRotation, RightPoleTarget, up );
// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML
// docs; kept defensively, matching TwoBoneIK/LookAtIK's already-verified usage.
#pragma warning disable CS0612
Renderer.PostAnimationUpdate();
#pragma warning restore CS0612
}
// Plant mode (opt-in via PlantToGround): correct each foot's hover WITHOUT moving the pelvis.
// Each foot's plant point (the ball bone, or the leg end when unset) is measured against the
// ground; its trailing-minimum height over PlantWindowSeconds is the planted level, and the
// foot is lowered by exactly that hover (down to PlantHeightOffset) with its authored rotation
// kept. Authored step lifts and heel raises above the planted level ride on top untouched;
// flat-authored feet come out flat; a grounded foot is never raised. The pelvis is left alone:
// a shared full-body offset belongs on the model root, not here.
private void SolvePlant()
{
_plantWindowL ??= new PlantWindow( PlantWindowSeconds );
_plantWindowR ??= new PlantWindow( PlantWindowSeconds );
_plantWindowL.WindowSeconds = PlantWindowSeconds;
_plantWindowR.WindowSeconds = PlantWindowSeconds;
Vector3 up = Vector3.Up;
Vector3 origin = Renderer.WorldPosition;
float now = Time.Now;
float dt = Time.Delta;
Renderer.TryGetBoneTransformAnimation( in _leftRoot, out var leftRootTx );
Renderer.TryGetBoneTransformAnimation( in _leftMid, out var leftMidTx );
Renderer.TryGetBoneTransformAnimation( in _leftEnd, out var leftEndTx );
Renderer.TryGetBoneTransformAnimation( in _rightRoot, out var rightRootTx );
Renderer.TryGetBoneTransformAnimation( in _rightMid, out var rightMidTx );
Renderer.TryGetBoneTransformAnimation( in _rightEnd, out var rightEndTx );
var leftPlantBone = _leftPlantResolved ? _leftPlant : _leftEnd;
var rightPlantBone = _rightPlantResolved ? _rightPlant : _rightEnd;
Renderer.TryGetBoneTransformAnimation( in leftPlantBone, out var leftPlantTx );
Renderer.TryGetBoneTransformAnimation( in rightPlantBone, out var rightPlantTx );
_smoothPlantL = SolvePlantCorrection( leftPlantTx.Position, up, origin, now, dt, _plantWindowL, _smoothPlantL,
out bool leftGrounded, out float leftResidual );
_smoothPlantR = SolvePlantCorrection( rightPlantTx.Position, up, origin, now, dt, _plantWindowR, _smoothPlantR,
out bool rightGrounded, out float rightResidual );
LeftGrounded = leftGrounded;
RightGrounded = rightGrounded;
LastPlantResidualL = leftResidual;
LastPlantResidualR = rightResidual;
CurrentPlantCorrectionL = _smoothPlantL;
CurrentPlantCorrectionR = _smoothPlantR;
CurrentPelvisOffset = 0f;
// A settled zero correction skips the leg write entirely (the target IS the animated pose,
// so the write is an unforced no-op - the same hazard the terrain path guards against).
if ( _smoothPlantL != 0f )
SolveLeg( in _leftRoot, in _leftMid, in _leftEnd, leftRootTx, leftMidTx, leftEndTx, _leftBindPose,
Vector3.Zero, -_smoothPlantL, leftEndTx.Rotation.ToNumerics(), LeftPoleTarget, up );
if ( _smoothPlantR != 0f )
SolveLeg( in _rightRoot, in _rightMid, in _rightEnd, rightRootTx, rightMidTx, rightEndTx, _rightBindPose,
Vector3.Zero, -_smoothPlantR, rightEndTx.Rotation.ToNumerics(), RightPoleTarget, up );
#pragma warning disable CS0612
Renderer.PostAnimationUpdate();
#pragma warning restore CS0612
}
// One foot's smoothed downward plant correction (world units, positive = foot lowered). Reads
// the plant point against the ground (a flat plane or a downward trace), pushes its height into
// the trailing-minimum window, removes the trailing-minimum hover down to PlantHeightOffset,
// and smooths the result. Returns 0 when no ground reference is available (nothing to plant to).
private float SolvePlantCorrection( Vector3 plantPos, Vector3 up, Vector3 origin, float now, float dt,
PlantWindow window, float smoothPrev, out bool grounded, out float residual )
{
float groundLevel;
if ( UseFlatGround )
{
grounded = true;
groundLevel = FlatGroundHeight;
}
else
{
var tr = TraceFoot( plantPos, up, origin );
grounded = tr.Hit;
groundLevel = tr.Hit ? Vector3.Dot( tr.HitPosition, up ) : 0f;
}
// Height of the plant point above the ground along the up axis (the hover to remove).
residual = Vector3.Dot( plantPos, up ) - groundLevel;
float target = 0f;
if ( grounded )
{
float trailingMin = window.Push( now, residual );
target = PlantToGroundSolver.ComputeCorrection( trailingMin, PlantHeightOffset, PlantMaxCorrection );
}
float smoothed = FootPlacementSolver.SmoothOffset( smoothPrev, target, SmoothingRate, dt );
if ( target == 0f && MathF.Abs( smoothed ) < 1e-3f )
smoothed = 0f;
return smoothed;
}
private void SolveLeg( in BoneCollection.Bone rootBone, in BoneCollection.Bone midBone, in BoneCollection.Bone endBone,
global::Transform rootTx, global::Transform midTx, global::Transform endTx, BindPoseData bindPose,
Vector3 pelvisShift, float footDelta, System.Numerics.Quaternion footTargetRotation, GameObject? poleTarget, Vector3 up )
{
Vector3 shiftedRoot = rootTx.Position + pelvisShift;
Vector3 shiftedMid = midTx.Position + pelvisShift;
Vector3 shiftedEnd = endTx.Position + pelvisShift;
Vector3 poleHint = poleTarget is not null && poleTarget.IsValid
? poleTarget.WorldPosition - shiftedRoot
: rootTx.Rotation * bindPose.DefaultPoleDirection.ToSandbox();
var input = new TwoBoneIkInput
{
RootPosition = shiftedRoot.ToNumerics(),
MidPosition = shiftedMid.ToNumerics(),
EndPosition = shiftedEnd.ToNumerics(),
RootRotation = rootTx.Rotation.ToNumerics(),
MidRotation = midTx.Rotation.ToNumerics(),
EndRotation = endTx.Rotation.ToNumerics(),
TargetPosition = (endTx.Position + up * footDelta).ToNumerics(),
TargetRotation = footTargetRotation,
HasPole = true,
PoleHint = poleHint.ToNumerics(),
PoleAngleOffsetRadians = 0f,
FallbackBendNormal = bindPose.BendNormal,
PositionWeight = 1f,
RotationWeight = FootRotationWeight,
MasterWeight = Weight,
SoftFraction = 0f,
MaxStretch = 0f,
};
var result = TwoBoneIkSolver.Solve( input );
// SetBoneTransform expects model-local space, unlike TryGetBoneTransformAnimation/
// TryGetBoneTransform which are documented as worldspace - see MathBridge.ToModelLocal.
Renderer!.SetBoneTransform( in rootBone, Renderer.ToModelLocal( new global::Transform( shiftedRoot, result.RootRotation.ToSandbox() ).WithScale( rootTx.Scale ) ) );
Renderer.SetBoneTransform( in midBone, Renderer.ToModelLocal( new global::Transform( result.MidPosition.ToSandbox(), result.MidRotation.ToSandbox() ).WithScale( midTx.Scale ) ) );
Renderer.SetBoneTransform( in endBone, Renderer.ToModelLocal( new global::Transform( result.EndPosition.ToSandbox(), result.EndRotation.ToSandbox() ).WithScale( endTx.Scale ) ) );
}
private (bool Hit, Vector3 HitPosition, Vector3 Normal) TraceFoot( Vector3 footPos, Vector3 up, Vector3 origin )
{
const float traceMargin = 2f;
Vector3 footOnPlane = footPos - up * Vector3.Dot( footPos - origin, up );
Vector3 traceStart = footOnPlane + up * (MaxStepUp + traceMargin);
Vector3 traceEnd = footOnPlane - up * (MaxStepDown + traceMargin);
var trace = Scene.Trace.FromTo( traceStart, traceEnd ).IgnoreGameObjectHierarchy( Renderer!.GameObject.Root );
var tags = IgnoreTags.Split( ' ', StringSplitOptions.RemoveEmptyEntries );
if ( tags.Length > 0 )
trace = trace.WithoutTags( tags );
var tr = trace.Run();
return (tr.Hit, tr.HitPosition, tr.Normal);
}
[MemberNotNullWhen( true, nameof( Renderer ) )]
private bool EnsureResolved()
{
if ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( LeftFootBone ) || string.IsNullOrEmpty( RightFootBone ) )
return false;
var signature = (Renderer, LeftFootBone, RightFootBone, LeftRootOverride, LeftMidOverride, RightRootOverride, RightMidOverride, PelvisBoneOverride, LeftPlantBone, RightPlantBone);
if ( signature.Equals( _cachedSignature ) && _leftRoot is not null )
return true;
var bones = Renderer.Model.Bones;
if ( !TryResolveLeg( bones, LeftFootBone, LeftRootOverride, LeftMidOverride, out var leftRootNode, out _leftRoot, out _leftMid, out _leftEnd, out _leftBindPose ) )
return false;
if ( !TryResolveLeg( bones, RightFootBone, RightRootOverride, RightMidOverride, out var rightRootNode, out _rightRoot, out _rightMid, out _rightEnd, out _rightBindPose ) )
return false;
ResolvePelvis( bones, leftRootNode, rightRootNode );
ResolvePlantBones( bones );
_cachedSignature = signature;
_smoothPelvis = 0f;
_smoothDeltaL = 0f;
_smoothDeltaR = 0f;
_smoothPlantL = 0f;
_smoothPlantR = 0f;
_plantWindowL?.Reset();
_plantWindowR?.Reset();
return true;
}
// Plant points (ball / toe bone) whose ground contact is planted. Optional: an unset or
// absent name falls back to the leg's own end bone at solve time (_*PlantResolved = false).
private void ResolvePlantBones( BoneCollection bones )
{
_leftPlantResolved = false;
_rightPlantResolved = false;
if ( !string.IsNullOrEmpty( LeftPlantBone ) && bones.HasBone( LeftPlantBone ) )
{
_leftPlant = bones.GetBone( LeftPlantBone );
_leftPlantResolved = true;
}
if ( !string.IsNullOrEmpty( RightPlantBone ) && bones.HasBone( RightPlantBone ) )
{
_rightPlant = bones.GetBone( RightPlantBone );
_rightPlantResolved = true;
}
}
private static bool TryResolveLeg( BoneCollection bones, string endBoneName, string rootOverrideName, string midOverrideName,
out IBoneNode? rootNode, out BoneCollection.Bone rootBone, out BoneCollection.Bone midBone, out BoneCollection.Bone endBone,
out BindPoseData bindPose )
{
rootNode = null;
rootBone = null!;
midBone = null!;
endBone = null!;
bindPose = default;
if ( !bones.HasBone( endBoneName ) )
return false;
IBoneNode endNode = new SandboxBoneNode( bones.GetBone( endBoneName ) );
IBoneNode? rootOverrideNode = null;
if ( !string.IsNullOrEmpty( rootOverrideName ) )
{
if ( !bones.HasBone( rootOverrideName ) )
return false;
rootOverrideNode = new SandboxBoneNode( bones.GetBone( rootOverrideName ) );
}
IBoneNode? midOverrideNode = null;
if ( !string.IsNullOrEmpty( midOverrideName ) )
{
if ( !bones.HasBone( midOverrideName ) )
return false;
midOverrideNode = new SandboxBoneNode( bones.GetBone( midOverrideName ) );
}
var chain = BoneChainResolver.Resolve( endNode, rootOverrideNode, midOverrideNode );
if ( !chain.Success )
return false;
rootNode = chain.Root;
rootBone = ((SandboxBoneNode)chain.Root!).Bone;
midBone = ((SandboxBoneNode)chain.Mid!).Bone;
endBone = ((SandboxBoneNode)chain.End!).Bone;
var rootBindWorld = global::Transform.Zero;
var midBindWorld = global::Transform.Concat( rootBindWorld, midBone.LocalTransform );
var endBindWorld = global::Transform.Concat( midBindWorld, endBone.LocalTransform );
bindPose = TwoBoneIkSolver.AnalyzeBindPose(
rootBindWorld.Position.ToNumerics(),
midBindWorld.Position.ToNumerics(),
endBindWorld.Position.ToNumerics() );
return true;
}
private void ResolvePelvis( BoneCollection bones, IBoneNode? leftRootNode, IBoneNode? rightRootNode )
{
if ( !string.IsNullOrEmpty( PelvisBoneOverride ) )
{
if ( bones.HasBone( PelvisBoneOverride ) )
{
var pelvisBone = bones.GetBone( PelvisBoneOverride );
if ( !IsChainBone( pelvisBone.Name ) )
{
_pelvisBone = pelvisBone;
PelvisResolved = true;
return;
}
}
PelvisResolved = false;
return;
}
if ( leftRootNode is not null && rightRootNode is not null )
{
var ancestor = BoneChainResolver.FindCommonAncestor( leftRootNode, rightRootNode );
if ( ancestor is not null && !IsChainBone( ancestor.Name ) && bones.HasBone( ancestor.Name ) )
{
_pelvisBone = bones.GetBone( ancestor.Name );
PelvisResolved = true;
return;
}
}
PelvisResolved = false;
}
private bool IsChainBone( string name )
=> name == _leftRoot.Name || name == _leftMid.Name || name == _leftEnd.Name
|| name == _rightRoot.Name || name == _rightMid.Name || name == _rightEnd.Name;
protected override void DrawGizmos()
{
if ( !EnsureResolved() )
{
Gizmo.Draw.Color = Color.Red;
Gizmo.Draw.WorldText( "FootPlacementIK: invalid leg chains", new global::Transform( WorldPosition ) );
return;
}
Renderer.TryGetBoneTransformAnimation( in _leftEnd, out var leftEndTx );
Renderer.TryGetBoneTransformAnimation( in _rightEnd, out var rightEndTx );
Vector3 up = Vector3.Up;
Vector3 origin = Renderer.WorldPosition;
DrawFootGizmo( leftEndTx.Position, up, origin, _smoothDeltaL );
DrawFootGizmo( rightEndTx.Position, up, origin, _smoothDeltaR );
if ( PelvisResolved )
{
Renderer.TryGetBoneTransformAnimation( in _pelvisBone, out var pelvisTx );
if ( MathF.Abs( _smoothPelvis ) > 0.01f )
{
Gizmo.Draw.Color = Color.Magenta;
Gizmo.Draw.Arrow( pelvisTx.Position, pelvisTx.Position + up * _smoothPelvis, 2f, 1f );
}
}
else
{
Gizmo.Draw.Color = Color.Orange;
Gizmo.Draw.WorldText( "FootPlacementIK: no pelvis resolved, feet-only mode (see PelvisBoneOverride)", new global::Transform( WorldPosition + Vector3.Up * 8f ) );
}
}
private void DrawFootGizmo( Vector3 footPos, Vector3 up, Vector3 origin, float smoothDelta )
{
var trace = TraceFoot( footPos, up, origin );
const float traceMargin = 2f;
Vector3 footOnPlane = footPos - up * Vector3.Dot( footPos - origin, up );
Vector3 traceStart = footOnPlane + up * (MaxStepUp + traceMargin);
Vector3 traceEnd = footOnPlane - up * (MaxStepDown + traceMargin);
Gizmo.Draw.Color = trace.Hit ? Color.Green : Color.Red;
Gizmo.Draw.Line( traceStart, traceEnd );
if ( trace.Hit )
{
Gizmo.Draw.LineSphere( new Sphere( trace.HitPosition, 1.5f ) );
Gizmo.Draw.Arrow( trace.HitPosition, trace.HitPosition + trace.Normal * 6f, 1.5f, 1f );
Gizmo.Draw.Color = Color.Cyan;
Gizmo.Draw.LineSphere( new Sphere( footPos + up * smoothDelta, 1.5f ) );
}
}
}
Game
library
namespace BetterIk.Maths;
using Vector3 = System.Numerics.Vector3;
/// <summary>Per-frame input to <see cref="FootPlacementSolver.Solve"/>. Angles are radians.</summary>
public struct FootPlacementInput
{
public FootInput LeftFoot;
public FootInput RightFoot;
/// <summary>Character ground-plane reference (typically the model root world position).</summary>
public Vector3 OriginPosition;
/// <summary>World up. Normalized by the solver.</summary>
public Vector3 UpAxis;
/// <summary>Additive raise applied to every grounded foot's delta.</summary>
public float FootHeightOffset;
/// <summary>Max positive (upward) vertical correction per foot.</summary>
public float MaxStepUp;
/// <summary>Max negative (downward) vertical correction per foot, as a positive number.</summary>
public float MaxStepDown;
/// <summary>Max pelvis drop, as a positive number.</summary>
public float MaxPelvisDrop;
/// <summary>Max pelvis raise, usually 0.</summary>
public float MaxPelvisRaise;
/// <summary>Clamp on how far a foot's rotation may tilt to align with the ground normal.</summary>
public float MaxFootRotationRadians;
/// <summary>Ground hits steeper than this (measured from up) are treated as no-hit.</summary>
public float MaxGroundSlopeRadians;
/// <summary>0-1. Applied ONLY to <see cref="FootPlacementResult.PelvisOffset"/> - per-foot
/// results are unweighted, so the caller's own two-bone solve (with its own tested,
/// endpoint-exact weight blend) is the single place feet actually get blended.</summary>
public float Weight;
}
Game
library
namespace BetterIk.Maths;
/// <summary>Output of <see cref="FootPlacementSolver.Solve"/>.</summary>
public struct FootPlacementResult
{
public FootResult LeftFoot;
public FootResult RightFoot;
/// <summary>Clamped AND weight-scaled, measured along the up axis. Final, write-ready -
/// unlike the per-foot results, this already has <see cref="FootPlacementInput.Weight"/>
/// applied (see that type's doc comment for why the split is asymmetric).</summary>
public float PelvisOffset;
/// <summary>False only when <see cref="FootPlacementInput.UpAxis"/> is degenerate.</summary>
public bool Solved;
}
Game
library
#nullable enable annotations // Global usings for the s&box in-engine compiler. // // The plain net8.0 dev harness gets these automatically via <ImplicitUsings>, // but s&box's compiler injects no BCL usings at all - without this file the // library fails to compile inside the editor. global using System; global using System.Collections.Generic; global using System.Linq; // NOTE on Vector3/Quaternion: s&box declares its own Vector3 and Rotation in the // global namespace, which wins over `using System.Numerics;` during name lookup. // The engine-agnostic math in Maths/ uses System.Numerics types exclusively so it // also compiles in the plain net8.0 dev harness (dev/BetterIk.Dev.csproj). Every // file under Maths/ that uses the simple name Vector3 or Quaternion carries a // namespace-scoped alias after its file-scoped namespace line, e.g.: // // namespace BetterIk.Maths; // using Vector3 = System.Numerics.Vector3; // // A global alias does not work here (CS0576: conflicts with the global-namespace // type at every use site), the alias must be namespace-scoped per file.
Game
library
namespace BetterIk.Maths;
using Vector3 = System.Numerics.Vector3;
using Quaternion = System.Numerics.Quaternion;
/// <summary>
/// Engine-agnostic, unconstrained FABRIK solver for variable-length chains (tails, tentacles,
/// ropes). No pole vectors, no joint constraints/limits, no per-joint stiffness in this version -
/// use TwoBoneIK for hinge-like limb behavior; this is for chains that don't need it. Pure and
/// stateless: identical input always produces identical output, safe to call every frame.
/// </summary>
public static class FabrikSolver
{
private const float TinyLenSq = 1e-12f;
public static FabrikResult Solve(in FabrikInput input)
{
Vector3[] animated = input.JointPositions;
int n = animated.Length;
float weight = Math.Clamp(input.Weight, 0f, 1f);
// Structural early-out: exact passthrough, no solve runs at all.
if (weight <= 0f)
{
return new FabrikResult
{
JointPositions = CopyArray(animated),
IterationsUsed = 0,
Converged = false,
};
}
float[] segLengths = new float[n - 1];
float totalLength = 0f;
for (int i = 0; i < n - 1; i++)
{
segLengths[i] = (animated[i + 1] - animated[i]).Length();
totalLength += segLengths[i];
}
Vector3 root = animated[0];
Vector3 toTarget = input.TargetPosition - root;
float rootToTargetDist = toTarget.Length();
Vector3[] solved;
int iterationsUsed;
bool converged;
// Target coincides with root: aim direction is undefined, passthrough unchanged rather
// than dividing by a zero-length vector.
if (rootToTargetDist < 1e-5f)
{
solved = CopyArray(animated);
iterationsUsed = 0;
converged = false;
}
else if (rootToTargetDist >= totalLength)
{
// Unreachable: analytic straight chain along the root-to-target ray, deterministic.
Vector3 dir = toTarget / rootToTargetDist;
solved = new Vector3[n];
solved[0] = root;
float cumulative = 0f;
for (int i = 1; i < n; i++)
{
cumulative += segLengths[i - 1];
solved[i] = root + dir * cumulative;
}
iterationsUsed = 0;
converged = false;
}
else
{
solved = CopyArray(animated);
float tolerance = MathF.Max(input.Tolerance, 1e-6f);
int maxIterations = Math.Max(input.MaxIterations, 1);
converged = false;
iterationsUsed = 0;
for (int iter = 0; iter < maxIterations; iter++)
{
iterationsUsed = iter + 1;
// Backward pass: pin the end to the target, walk toward the root re-fixing lengths.
solved[n - 1] = input.TargetPosition;
for (int i = n - 2; i >= 0; i--)
{
Vector3 dir = SafeDirection(solved[i] - solved[i + 1]);
solved[i] = solved[i + 1] + dir * segLengths[i];
}
// Forward pass: re-pin the root, walk toward the end re-fixing lengths.
solved[0] = root;
for (int i = 1; i < n; i++)
{
Vector3 dir = SafeDirection(solved[i] - solved[i - 1]);
solved[i] = solved[i - 1] + dir * segLengths[i - 1];
}
if ((solved[n - 1] - input.TargetPosition).Length() <= tolerance)
{
converged = true;
break;
}
}
}
if (weight >= 1f)
{
return new FabrikResult { JointPositions = solved, IterationsUsed = iterationsUsed, Converged = converged };
}
// Position-lerp blend. This does not preserve segment lengths at intermediate weights -
// accepted tradeoff, same class as TwoBoneIK's weight blend; tails are visually forgiving.
var blended = new Vector3[n];
for (int i = 0; i < n; i++)
blended[i] = Vector3.Lerp(animated[i], solved[i], weight);
return new FabrikResult { JointPositions = blended, IterationsUsed = iterationsUsed, Converged = converged };
}
/// <summary>
/// Derives per-joint world rotations from the change in segment direction between the animated
/// and solved poses, via the same shortest-arc delta-rotation philosophy already proven under
/// bone roll (IkMath.FromToRotation). No twist/roll control in this version - long chains under
/// large deflection can accumulate visually odd roll; acceptable for v1, a twist-distribution
/// pass is a possible later addition. The leaf bone (last index) has no segment of its own, so
/// it is a documented convention, not a derived truth: it reuses the last real segment's delta.
/// </summary>
public static Quaternion[] DeriveRotations(Vector3[] animatedPositions, Vector3[] solvedPositions, Quaternion[] animatedRotations)
{
int n = animatedPositions.Length;
var result = new Quaternion[n];
Quaternion lastDelta = Quaternion.Identity;
for (int i = 0; i < n - 1; i++)
{
Vector3 animatedDir = SafeDirection(animatedPositions[i + 1] - animatedPositions[i]);
Vector3 solvedDir = SafeDirection(solvedPositions[i + 1] - solvedPositions[i]);
lastDelta = IkMath.FromToRotation(animatedDir, solvedDir);
result[i] = Quaternion.Normalize(lastDelta * animatedRotations[i]);
}
// Leaf bone convention: reuse the last real segment's delta, applied to the leaf's own
// animated rotation (not the previous joint's).
result[n - 1] = Quaternion.Normalize(lastDelta * animatedRotations[n - 1]);
return result;
}
private static Vector3 SafeDirection(Vector3 v)
{
float lenSq = v.LengthSquared();
return lenSq < TinyLenSq ? Vector3.UnitX : v / MathF.Sqrt(lenSq);
}
// Array.Clone() is not on s&box's code whitelist for the game-code assembly (confirmed via
// a live compile attempt: "System.Array.Clone() is not allowed when whitelist is enabled").
// A manual element-by-element copy is the whitelist-safe equivalent.
private static Vector3[] CopyArray(Vector3[] source)
{
var copy = new Vector3[source.Length];
for (int i = 0; i < source.Length; i++)
copy[i] = source[i];
return copy;
}
}
Game
library
namespace BetterIk.Maths;
using Vector3 = System.Numerics.Vector3;
using Quaternion = System.Numerics.Quaternion;
/// <summary>
/// Engine-agnostic, closed-form two-bone IK solver with pole vector control. Pure and stateless:
/// identical input always produces identical output, safe to call every frame for runtime blending.
/// </summary>
public static class TwoBoneIkSolver
{
// All thresholds below are relative to chain length (Lmax) or another geometric quantity,
// never absolute, so the solver is scale-equivariant.
private const float TinyRel = 1e-6f;
public static BindPoseData AnalyzeBindPose(Vector3 rootPos, Vector3 midPos, Vector3 endPos)
{
float l1 = (midPos - rootPos).Length();
float l2 = (endPos - midPos).Length();
float lengthSum = MathF.Max(l1 + l2, 1e-8f);
Vector3 chainDir = IkMath.SafeNormalize(endPos - rootPos, Vector3.UnitX);
Vector3 elbowOffset = IkMath.ProjectPerpendicular(midPos - rootPos, chainDir);
float threshold = 1e-4f * lengthSum;
if (elbowOffset.Length() >= threshold)
{
Vector3 poleDir = Vector3.Normalize(elbowOffset);
Vector3 bendNormal = IkMath.SafeNormalize(
Vector3.Cross(endPos - rootPos, midPos - rootPos),
IkMath.AnyPerpendicular(chainDir));
return new BindPoseData(l1, l2, bendNormal, poleDir, true);
}
else
{
Vector3 poleDir = IkMath.AnyPerpendicular(chainDir);
Vector3 bendNormal = Vector3.Normalize(Vector3.Cross(chainDir, poleDir));
return new BindPoseData(l1, l2, bendNormal, poleDir, false);
}
}
public static TwoBoneIkResult Solve(in TwoBoneIkInput input)
{
Vector3 a = input.RootPosition;
Vector3 b = input.MidPosition;
Vector3 c = input.EndPosition;
float l1 = (b - a).Length();
float l2 = (c - b).Length();
float lmax = l1 + l2;
float wMaster = Math.Clamp(input.MasterWeight, 0f, 1f);
float wPos = wMaster * Math.Clamp(input.PositionWeight, 0f, 1f);
float wRot = wMaster * Math.Clamp(input.RotationWeight, 0f, 1f);
Quaternion endRotation = Quaternion.Normalize(BlendRotation(input.EndRotation, input.TargetRotation, wRot));
// Degenerate chain: at least one bone effectively zero length. Rotation goal is independent
// of chain geometry, so it still applies; position/root/mid pass through unmodified.
if (lmax < 1e-8f || l1 < TinyRel * lmax || l2 < TinyRel * lmax)
{
return new TwoBoneIkResult
{
RootRotation = input.RootRotation,
MidRotation = input.MidRotation,
EndRotation = endRotation,
MidPosition = b,
EndPosition = c,
AppliedStretch = 1f,
Solved = false,
};
}
Vector3 toTarget = input.TargetPosition - a;
float d = toTarget.Length();
// Target coincides with root: root-to-target direction is undefined, fall back to the
// animated chain direction (never NaN; continuity through this exact point is not required).
Vector3 aHat = d < TinyRel * lmax
? IkMath.SafeNormalize(c - a, Vector3.UnitX)
: toTarget / d;
(float dEff, float sFull, float dFinal) = SolveReach(d, lmax, input.SoftFraction, input.MaxStretch);
float l1s = sFull * l1;
float l2s = sFull * l2;
// Near-side clamp: when bone lengths differ, the chain also cannot reach closer than
// |l1s - l2s| (folding the longer bone back over the shorter one). Mirrors the far-side
// max-reach clamp; without it cosAlpha blows outside [-1,1] and the elbow snaps to an
// inconsistent pose whose end position drifts off target. A near-side reach clamp found
// by fuzz testing, not covered by the original edge-case table (which only listed
// "beyond max reach" and "exactly at zero").
float minReach = MathF.Abs(l1s - l2s);
if (dFinal < minReach)
dFinal = minReach;
Vector3 dHat = SelectElbowDirection(input, a, b, aHat, lmax);
float alpha = SolveRootAngle(dFinal, l1s, l2s, lmax);
Vector3 midSolved = a + l1s * (MathF.Cos(alpha) * aHat + MathF.Sin(alpha) * dHat);
Vector3 endSolved = a + dFinal * aHat;
Vector3 rawPoseNormal = Vector3.Cross(c - a, b - a);
Vector3 oldNormalHint = rawPoseNormal.LengthSquared() >= (1e-5f * l1 * l2) * (1e-5f * l1 * l2)
? rawPoseNormal
: input.FallbackBendNormal;
Vector3 newNormalHint = Vector3.Cross(aHat, dHat);
Quaternion deltaRootFull = IkMath.DeltaRotation(b - a, oldNormalHint, midSolved - a, newNormalHint);
Quaternion deltaMidFull = IkMath.DeltaRotation(c - b, oldNormalHint, endSolved - midSolved, newNormalHint);
Quaternion deltaRoot = BlendRotation(Quaternion.Identity, deltaRootFull, wPos);
Quaternion deltaMid = BlendRotation(Quaternion.Identity, deltaMidFull, wPos);
float sBlended = 1f + (sFull - 1f) * wPos;
Vector3 midBlended = a + sBlended * Vector3.Transform(b - a, deltaRoot);
Vector3 endBlended = midBlended + sBlended * Vector3.Transform(c - b, deltaMid);
return new TwoBoneIkResult
{
RootRotation = Quaternion.Normalize(deltaRoot * input.RootRotation),
MidRotation = Quaternion.Normalize(deltaMid * input.MidRotation),
EndRotation = endRotation,
MidPosition = midBlended,
EndPosition = endBlended,
AppliedStretch = sBlended,
Solved = true,
};
}
// Exact at t=0/t=1 (no Slerp numerical noise at the endpoints); Slerp in between.
private static Quaternion BlendRotation(Quaternion from, Quaternion to, float t)
{
if (t <= 0f) return from;
if (t >= 1f) return to;
return Quaternion.Slerp(from, to, t);
}
// Soft clamp (exponential falloff) + stretch. Returns the reach distance actually solved for
// (dEff), the blended-in stretch scale (sFull), and the final root-to-end distance (dFinal).
private static (float dEff, float sFull, float dFinal) SolveReach(float d, float lmax, float softFractionRaw, float maxStretchRaw)
{
float softFraction = Math.Clamp(softFractionRaw, 0f, 0.499f);
float maxStretch = MathF.Max(maxStretchRaw, 0f);
float dEff;
if (softFraction < 1e-4f)
{
dEff = MathF.Min(d, lmax);
}
else
{
float dSoft = (1f - softFraction) * lmax;
float r = softFraction * lmax;
dEff = d <= dSoft ? d : dSoft + r * (1f - MathF.Exp(-(d - dSoft) / r));
}
float sFull = dEff > 0f ? Math.Clamp(d / dEff, 1f, 1f + maxStretch) : 1f;
float dFinal = MathF.Min(d, dEff * sFull);
return (dEff, sFull, dFinal);
}
// Law of cosines for the angle at the root, guarded against the near-zero-reach fold singularity
// (where the denominator would be zero); alpha = pi/2 there safely folds the chain via dHat.
private static float SolveRootAngle(float dFinal, float l1s, float l2s, float lmax)
{
if (dFinal < TinyRel * lmax || l1s < TinyRel * lmax)
return MathF.PI / 2f;
float cosAlpha = (dFinal * dFinal + l1s * l1s - l2s * l2s) / (2f * dFinal * l1s);
return MathF.Acos(Math.Clamp(cosAlpha, -1f, 1f));
}
// Pole hint -> pose-derived elbow offset -> fallback bend normal -> arbitrary perpendicular.
private static Vector3 SelectElbowDirection(in TwoBoneIkInput input, Vector3 a, Vector3 b, Vector3 aHat, float lmax)
{
if (input.HasPole)
{
Vector3 pPerp = IkMath.ProjectPerpendicular(input.PoleHint, aHat);
float threshold = 1e-4f * MathF.Max(input.PoleHint.Length(), lmax);
if (pPerp.Length() >= threshold)
return ApplyOffset(Vector3.Normalize(pPerp), aHat, input.PoleAngleOffsetRadians);
}
Vector3 mPerp = IkMath.ProjectPerpendicular(b - a, aHat);
if (mPerp.Length() >= 1e-5f * lmax)
return ApplyOffset(Vector3.Normalize(mPerp), aHat, input.PoleAngleOffsetRadians);
// FallbackBendNormal is a plane normal, not an in-plane direction: cross with aHat to get
// the in-plane, perpendicular-to-aHat elbow direction it implies.
Vector3 crossFallback = Vector3.Cross(input.FallbackBendNormal, aHat);
Vector3 dHat = crossFallback.LengthSquared() >= 1e-10f
? Vector3.Normalize(crossFallback)
: IkMath.AnyPerpendicular(aHat);
return ApplyOffset(dHat, aHat, input.PoleAngleOffsetRadians);
}
private static Vector3 ApplyOffset(Vector3 dHat, Vector3 axis, float angleRadians)
{
if (angleRadians == 0f)
return dHat;
return Vector3.Transform(dHat, Quaternion.CreateFromAxisAngle(axis, angleRadians));
}
}
Game
library
#nullable enable
using Sandbox;
using BetterIk.Skeleton;
namespace BetterIk;
/// <summary>Thin adapter over the real engine BoneCollection.Bone, exposing just enough to
/// let BoneChainResolver walk the skeleton. No logic beyond delegation - not unit tested.</summary>
internal sealed class SandboxBoneNode : IBoneNode
{
public SandboxBoneNode(BoneCollection.Bone bone)
{
Bone = bone;
}
public BoneCollection.Bone Bone { get; }
public string Name => Bone.Name;
public IBoneNode? Parent => Bone.Parent is null ? null : new SandboxBoneNode(Bone.Parent);
}
Game
library
#nullable enable
using System.Diagnostics.CodeAnalysis;
using System.Linq;
using Sandbox;
using BetterIk.Maths;
using BetterIk.Skeleton;
namespace BetterIk;
/// <summary>
/// Unconstrained FABRIK for variable-length chains (tails, tentacles, ropes). No pole vector, no
/// joint limits, no per-joint stiffness - for hinge-like limb IK, use TwoBoneIK instead. Drop on
/// the model root (or any descendant), set RootBone and EndBone to the chain's two ends; unlike
/// TwoBoneIK this cannot auto-walk a fixed depth, since a FABRIK chain's length is arbitrary, so
/// RootBone must be named explicitly.
/// </summary>
public sealed class FabrikIK : Component, IHasSkinnedRenderer
{
[Property] public SkinnedModelRenderer? Renderer { get; set; }
[Property, BoneName] public string RootBone { get; set; } = "";
[Property, BoneName] public string EndBone { get; set; } = "";
[Property] public GameObject? Target { get; set; }
[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;
[Property, Group( "Advanced" )] public int MaxIterations { get; set; } = 16;
// 0 = auto: 0.001 * total chain length, computed at solve time from the animated pose.
[Property, Group( "Advanced" )] public float Tolerance { get; set; } = 0f;
public bool HasValidChain { get; private set; }
public bool LastSolveApplied { get; private set; }
private (SkinnedModelRenderer Renderer, string RootBone, string EndBone) _cachedSignature;
// Root-first, end-last. Only valid once EnsureResolved() has returned true at least once.
private BoneCollection.Bone[] _chainBones = null!;
private Vector3[] _chainScales = null!;
protected override void OnStart()
{
Renderer ??= GameObject.GetComponent<SkinnedModelRenderer>()
?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );
}
protected override void OnPreRender()
{
Solve();
}
private void Solve()
{
if ( !EnsureResolved() )
{
HasValidChain = false;
LastSolveApplied = false;
return;
}
HasValidChain = true;
if ( Target is null || !Target.IsValid )
{
LastSolveApplied = false;
return;
}
// Weight <= 0 skips the read-solve-write cycle entirely rather than reading the animated
// pose and writing the identical value back. Live testing found the latter is NOT
// perfectly lossless through the world<->model-local round trip (Renderer.ToModelLocal)
// every frame: on Rig B's tail, repeatedly reading TryGetBoneTransformAnimation then
// writing the visually-unchanged result back via SetBoneTransform compounded into
// Infinity within a few seconds, even with bone scale frozen (Renderer.ToModelLocal
// finding above). Not writing at all when there is nothing to blend is strictly safer.
if ( Weight <= 0f )
{
LastSolveApplied = false;
return;
}
int n = _chainBones.Length;
var positions = new System.Numerics.Vector3[n];
var rotations = new System.Numerics.Quaternion[n];
float totalLength = 0f;
for ( int i = 0; i < n; i++ )
{
Renderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );
positions[i] = tx.Position.ToNumerics();
rotations[i] = tx.Rotation.ToNumerics();
if ( i > 0 )
totalLength += (positions[i] - positions[i - 1]).Length();
}
float tolerance = Tolerance > 0f ? Tolerance : 0.001f * MathF.Max( totalLength, 1e-4f );
var input = new FabrikInput
{
JointPositions = positions,
TargetPosition = Target.WorldPosition.ToNumerics(),
Weight = Weight,
MaxIterations = MaxIterations,
Tolerance = tolerance,
};
var result = FabrikSolver.Solve( input );
var solvedRotations = FabrikSolver.DeriveRotations( positions, result.JointPositions, rotations );
for ( int i = 0; i < n; i++ )
{
Renderer.SetBoneTransform( in _chainBones[i], Renderer.ToModelLocal(
new global::Transform( result.JointPositions[i].ToSandbox(), solvedRotations[i].ToSandbox() ).WithScale( _chainScales[i] ) ) );
}
// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML
// docs; kept defensively, matching the other components' already-verified usage.
#pragma warning disable CS0612
Renderer.PostAnimationUpdate();
#pragma warning restore CS0612
LastSolveApplied = true;
}
[MemberNotNullWhen( true, nameof( Renderer ) )]
private bool EnsureResolved()
{
if ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( RootBone ) || string.IsNullOrEmpty( EndBone ) )
return false;
var signature = (Renderer, RootBone, EndBone);
if ( signature.Equals( _cachedSignature ) && _chainBones is not null )
return true;
var bones = Renderer.Model.Bones;
if ( !bones.HasBone( EndBone ) )
return false;
IBoneNode endNode = new SandboxBoneNode( bones.GetBone( EndBone ) );
var chain = BoneChainResolver.ResolveNamedChain( endNode, RootBone );
if ( !chain.Success )
return false;
_chainBones = chain.Chain!.Select( node => ((SandboxBoneNode)node).Bone ).ToArray();
_cachedSignature = signature;
// Cached once here rather than re-read from TryGetBoneTransformAnimation every frame.
// Re-reading and re-writing world scale every frame round-trips it through
// Renderer.ToModelLocal on every solve; live testing found this compounds into Infinity
// within a few seconds on Rig B's tail (bone scale is not exactly 1 on that rig). IK never
// needs to change a bone's scale, so freezing it at first resolution is both safe and
// removes the feedback path entirely.
_chainScales = new Vector3[_chainBones.Length];
for ( int i = 0; i < _chainBones.Length; i++ )
{
Renderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );
_chainScales[i] = tx.Scale;
}
return true;
}
protected override void DrawGizmos()
{
if ( !EnsureResolved() )
{
Gizmo.Draw.Color = Color.Red;
Gizmo.Draw.WorldText( "FabrikIK: invalid chain", new global::Transform( WorldPosition ) );
return;
}
if ( Target is null || !Target.IsValid )
return;
Gizmo.Draw.Color = Color.Yellow;
for ( int i = 0; i < _chainBones.Length - 1; i++ )
{
Renderer.TryGetBoneTransform( in _chainBones[i], out var a );
Renderer.TryGetBoneTransform( in _chainBones[i + 1], out var b );
Gizmo.Draw.Line( a.Position, b.Position );
}
Gizmo.Draw.Color = Color.White;
Gizmo.Draw.LineSphere( new Sphere( Target.WorldPosition, 2f ) );
}
}
Game
library
#nullable enable annotations // Engine-agnostic IK math. No Sandbox/Editor references allowed in this folder: // these sources also compile in the plain net8.0 dev harness (dev/BetterIk.Dev.csproj) // so the solver can be unit-tested without the s&box editor running.
Game
library
namespace BetterIk.Maths;
using Quaternion = System.Numerics.Quaternion;
/// <summary>Per-foot output of <see cref="FootPlacementSolver.Solve"/>. Unweighted - the
/// aggregate <see cref="FootPlacementInput.Weight"/> is applied only to
/// <see cref="FootPlacementResult.PelvisOffset"/>, not here (see that type's doc comment).</summary>
public readonly struct FootResult
{
/// <summary>True when a hit existed, its normal was usable, and the slope was within limit.</summary>
public readonly bool Grounded;
/// <summary>Clamped, unweighted, measured along the up axis. 0 when not grounded.</summary>
public readonly float VerticalDelta;
/// <summary>Ground-aligned foot rotation goal, unweighted. Equals the input FootRotation
/// exactly when not grounded.</summary>
public readonly Quaternion TargetRotation;
public FootResult(bool grounded, float verticalDelta, Quaternion targetRotation)
{
Grounded = grounded;
VerticalDelta = verticalDelta;
TargetRotation = targetRotation;
}
}
Debug: View Raw JSON Response
{
"TotalCount": 75,
"Files": [
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/FabrikIK.cs",
"FileName": "FabrikIK.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nusing System.Diagnostics.CodeAnalysis;\r\nusing System.Linq;\r\nusing Sandbox;\r\nusing BetterIk.Maths;\r\nusing BetterIk.Skeleton;\r\n\r\nnamespace BetterIk;\r\n\r\n/// <summary>\r\n/// Unconstrained FABRIK for variable-length chains (tails, tentacles, ropes). No pole vector, no\r\n/// joint limits, no per-joint stiffness - for hinge-like limb IK, use TwoBoneIK instead. Drop on\r\n/// the model root (or any descendant), set RootBone and EndBone to the chain's two ends; unlike\r\n/// TwoBoneIK this cannot auto-walk a fixed depth, since a FABRIK chain's length is arbitrary, so\r\n/// RootBone must be named explicitly.\r\n/// </summary>\r\npublic sealed class FabrikIK : Component, IHasSkinnedRenderer\r\n{\r\n\t[Property] public SkinnedModelRenderer? Renderer { get; set; }\r\n\t[Property, BoneName] public string RootBone { get; set; } = \"\";\r\n\t[Property, BoneName] public string EndBone { get; set; } = \"\";\r\n\t[Property] public GameObject? Target { get; set; }\r\n\r\n\t[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;\r\n\r\n\t[Property, Group( \"Advanced\" )] public int MaxIterations { get; set; } = 16;\r\n\r\n\t// 0 = auto: 0.001 * total chain length, computed at solve time from the animated pose.\r\n\t[Property, Group( \"Advanced\" )] public float Tolerance { get; set; } = 0f;\r\n\r\n\tpublic bool HasValidChain { get; private set; }\r\n\tpublic bool LastSolveApplied { get; private set; }\r\n\r\n\tprivate (SkinnedModelRenderer Renderer, string RootBone, string EndBone) _cachedSignature;\r\n\t// Root-first, end-last. Only valid once EnsureResolved() has returned true at least once.\r\n\tprivate BoneCollection.Bone[] _chainBones = null!;\r\n\tprivate Vector3[] _chainScales = null!;\r\n\r\n\tprotected override void OnStart()\r\n\t{\r\n\t\tRenderer ??= GameObject.GetComponent<SkinnedModelRenderer>()\r\n\t\t\t?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );\r\n\t}\r\n\r\n\tprotected override void OnPreRender()\r\n\t{\r\n\t\tSolve();\r\n\t}\r\n\r\n\tprivate void Solve()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tHasValidChain = false;\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tHasValidChain = true;\r\n\r\n\t\tif ( Target is null || !Target.IsValid )\r\n\t\t{\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\t// Weight <= 0 skips the read-solve-write cycle entirely rather than reading the animated\r\n\t\t// pose and writing the identical value back. Live testing found the latter is NOT\r\n\t\t// perfectly lossless through the world<->model-local round trip (Renderer.ToModelLocal)\r\n\t\t// every frame: on Rig B's tail, repeatedly reading TryGetBoneTransformAnimation then\r\n\t\t// writing the visually-unchanged result back via SetBoneTransform compounded into\r\n\t\t// Infinity within a few seconds, even with bone scale frozen (Renderer.ToModelLocal\r\n\t\t// finding above). Not writing at all when there is nothing to blend is strictly safer.\r\n\t\tif ( Weight <= 0f )\r\n\t\t{\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tint n = _chainBones.Length;\r\n\t\tvar positions = new System.Numerics.Vector3[n];\r\n\t\tvar rotations = new System.Numerics.Quaternion[n];\r\n\r\n\t\tfloat totalLength = 0f;\r\n\t\tfor ( int i = 0; i < n; i++ )\r\n\t\t{\r\n\t\t\tRenderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );\r\n\t\t\tpositions[i] = tx.Position.ToNumerics();\r\n\t\t\trotations[i] = tx.Rotation.ToNumerics();\r\n\t\t\tif ( i > 0 )\r\n\t\t\t\ttotalLength += (positions[i] - positions[i - 1]).Length();\r\n\t\t}\r\n\r\n\t\tfloat tolerance = Tolerance > 0f ? Tolerance : 0.001f * MathF.Max( totalLength, 1e-4f );\r\n\r\n\t\tvar input = new FabrikInput\r\n\t\t{\r\n\t\t\tJointPositions = positions,\r\n\t\t\tTargetPosition = Target.WorldPosition.ToNumerics(),\r\n\t\t\tWeight = Weight,\r\n\t\t\tMaxIterations = MaxIterations,\r\n\t\t\tTolerance = tolerance,\r\n\t\t};\r\n\r\n\t\tvar result = FabrikSolver.Solve( input );\r\n\t\tvar solvedRotations = FabrikSolver.DeriveRotations( positions, result.JointPositions, rotations );\r\n\r\n\t\tfor ( int i = 0; i < n; i++ )\r\n\t\t{\r\n\t\t\tRenderer.SetBoneTransform( in _chainBones[i], Renderer.ToModelLocal(\r\n\t\t\t\tnew global::Transform( result.JointPositions[i].ToSandbox(), solvedRotations[i].ToSandbox() ).WithScale( _chainScales[i] ) ) );\r\n\t\t}\r\n\r\n\t\t// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML\r\n\t\t// docs; kept defensively, matching the other components' already-verified usage.\r\n#pragma warning disable CS0612\r\n\t\tRenderer.PostAnimationUpdate();\r\n#pragma warning restore CS0612\r\n\r\n\t\tLastSolveApplied = true;\r\n\t}\r\n\r\n\t[MemberNotNullWhen( true, nameof( Renderer ) )]\r\n\tprivate bool EnsureResolved()\r\n\t{\r\n\t\tif ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( RootBone ) || string.IsNullOrEmpty( EndBone ) )\r\n\t\t\treturn false;\r\n\r\n\t\tvar signature = (Renderer, RootBone, EndBone);\r\n\t\tif ( signature.Equals( _cachedSignature ) && _chainBones is not null )\r\n\t\t\treturn true;\r\n\r\n\t\tvar bones = Renderer.Model.Bones;\r\n\t\tif ( !bones.HasBone( EndBone ) )\r\n\t\t\treturn false;\r\n\r\n\t\tIBoneNode endNode = new SandboxBoneNode( bones.GetBone( EndBone ) );\r\n\t\tvar chain = BoneChainResolver.ResolveNamedChain( endNode, RootBone );\r\n\t\tif ( !chain.Success )\r\n\t\t\treturn false;\r\n\r\n\t\t_chainBones = chain.Chain!.Select( node => ((SandboxBoneNode)node).Bone ).ToArray();\r\n\t\t_cachedSignature = signature;\r\n\r\n\t\t// Cached once here rather than re-read from TryGetBoneTransformAnimation every frame.\r\n\t\t// Re-reading and re-writing world scale every frame round-trips it through\r\n\t\t// Renderer.ToModelLocal on every solve; live testing found this compounds into Infinity\r\n\t\t// within a few seconds on Rig B's tail (bone scale is not exactly 1 on that rig). IK never\r\n\t\t// needs to change a bone's scale, so freezing it at first resolution is both safe and\r\n\t\t// removes the feedback path entirely.\r\n\t\t_chainScales = new Vector3[_chainBones.Length];\r\n\t\tfor ( int i = 0; i < _chainBones.Length; i++ )\r\n\t\t{\r\n\t\t\tRenderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );\r\n\t\t\t_chainScales[i] = tx.Scale;\r\n\t\t}\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\tprotected override void DrawGizmos()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tGizmo.Draw.Color = Color.Red;\r\n\t\t\tGizmo.Draw.WorldText( \"FabrikIK: invalid chain\", new global::Transform( WorldPosition ) );\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tif ( Target is null || !Target.IsValid )\r\n\t\t\treturn;\r\n\r\n\t\tGizmo.Draw.Color = Color.Yellow;\r\n\t\tfor ( int i = 0; i < _chainBones.Length - 1; i++ )\r\n\t\t{\r\n\t\t\tRenderer.TryGetBoneTransform( in _chainBones[i], out var a );\r\n\t\t\tRenderer.TryGetBoneTransform( in _chainBones[i + 1], out var b );\r\n\t\t\tGizmo.Draw.Line( a.Position, b.Position );\r\n\t\t}\r\n\r\n\t\tGizmo.Draw.Color = Color.White;\r\n\t\tGizmo.Draw.LineSphere( new Sphere( Target.WorldPosition, 2f ) );\r\n\t}\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/Maths/FabrikResult.cs",
"FileName": "FabrikResult.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\n/// <summary>Output of <see cref=\"FabrikSolver.Solve\"/>.</summary>\r\npublic struct FabrikResult\r\n{\r\n /// <summary>Solved joint positions, world space, same length and root-first ordering as the input.</summary>\r\n public System.Numerics.Vector3[] JointPositions;\r\n\r\n public int IterationsUsed;\r\n\r\n /// <summary>True only if the end joint landed within Tolerance of the target. False for the\r\n /// unreachable-target clamp, the target-at-root passthrough, and any case that used every\r\n /// available iteration without reaching tolerance - not an error signal, just \"did not land\r\n /// exactly on target\", matching the geometric reality of a rigid chain.</summary>\r\n public bool Converged;\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/Skeleton/IBoneNode.cs",
"FileName": "IBoneNode.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nnamespace BetterIk.Skeleton;\r\n\r\n/// <summary>Minimal shape needed to walk a skeleton toward the root. Implemented by a real\r\n/// BoneCollection.Bone adapter in the engine layer, and by a fake node in tests.</summary>\r\npublic interface IBoneNode\r\n{\r\n string Name { get; }\r\n\r\n /// <summary>Null at the skeleton root.</summary>\r\n IBoneNode? Parent { get; }\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/IHasSkinnedRenderer.cs",
"FileName": "IHasSkinnedRenderer.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nusing Sandbox;\r\n\r\nnamespace BetterIk;\r\n\r\n/// <summary>Implemented by every IK component that operates on a SkinnedModelRenderer, so editor\r\n/// tooling (BoneNameControlWidget) can resolve the renderer to list bones without reflection.</summary>\r\npublic interface IHasSkinnedRenderer\r\n{\r\n\tSkinnedModelRenderer? Renderer { get; }\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/Maths/LookAtBindData.cs",
"FileName": "LookAtBindData.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\nusing Vector3 = System.Numerics.Vector3;\r\n\r\n/// <summary>Precomputed once per bone from the bind pose. Result of <see cref=\"LookAtSolver.AnalyzeBindPose\"/>.</summary>\r\npublic readonly struct LookAtBindData\r\n{\r\n /// <summary>Unit, bone-local space direction the bone should be treated as \"facing\".</summary>\r\n public readonly Vector3 LocalAimDirection;\r\n\r\n /// <summary>False if no usable child bone existed in the bind pose (leaf bone, or a degenerate\r\n /// child offset); <see cref=\"LocalAimDirection\"/> is still a valid unit vector, just an arbitrary\r\n /// deterministic fallback rather than an anatomically meaningful one.</summary>\r\n public readonly bool IsReliable;\r\n\r\n public LookAtBindData(Vector3 localAimDirection, bool isReliable)\r\n {\r\n LocalAimDirection = localAimDirection;\r\n IsReliable = isReliable;\r\n }\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": ".obj/__compiler_extra.cs",
"FileName": "__compiler_extra.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "global using static Sandbox.Internal.GlobalGameNamespace;\r\nglobal using Microsoft.AspNetCore.Components;\r\nglobal using Microsoft.AspNetCore.Components.Rendering;\r\n[assembly: global::System.Reflection.AssemblyMetadata( \"AddonTitle\", \"Better IK\" )]\r\n[assembly: global::System.Reflection.AssemblyMetadata( \"AddonIdent\", \"chomnr_better_ik\" )]\r\n[assembly: global::System.Reflection.AssemblyMetadata( \"OrgIdent\", \"notpointless\" )]\r\n[assembly: global::System.Reflection.AssemblyMetadata( \"Ident\", \"notpointless.chomnr_better_ik\" )]\r\n[assembly: global::System.Reflection.AssemblyMetadata( \"EngineVersion\", \"28\" )]\r\n[assembly: global::System.Reflection.AssemblyMetadata( \"EngineMinorVersion\", \"1\" )]\r\n\r\n[assembly: System.Runtime.Versioning.TargetFramework( \".NETCoreApp,Version=v9.0\", FrameworkDisplayName = \".NET 9.0\" )]\r\n[assembly: global::System.Reflection.AssemblyMetadata( \"CompileTime\", \"2026-07-18T00:05:12.5911669Z\" )]\r\n[assembly: global::System.Reflection.AssemblyVersion(\"0.0.119.0\")]\r\n[assembly: global::System.Reflection.AssemblyFileVersion(\"0.0.119.0\")]"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/Maths/PlantWindow.cs",
"FileName": "PlantWindow.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\n/// <summary>\r\n/// Trailing-minimum tracker over a fixed time window. Feed it one plant-point height per\r\n/// frame with the current time; <see cref=\"Push\"/> returns the lowest value seen within the\r\n/// last <see cref=\"WindowSeconds\"/>. Plant-to-ground foot placement uses it to find the\r\n/// \"planted\" level of a cyclic animation (the lowest the foot reached recently), so that only\r\n/// that constant is removed and authored lifts above it are preserved.\r\n///\r\n/// This is the one stateful helper in the Maths layer; the solvers themselves stay pure. It is\r\n/// engine-agnostic (floats and a time value only), so it is unit-testable without an editor,\r\n/// like the rest of the core. Samples are assumed pushed in non-decreasing time order (the\r\n/// engine wall-clock during a frame loop); the newest <see cref=\"Capacity\"/> samples are\r\n/// retained, so at very high frame rates a window longer than the buffer can hold is measured\r\n/// over as many recent samples as fit rather than the full duration.\r\n/// </summary>\r\npublic sealed class PlantWindow\r\n{\r\n private readonly float[] _time;\r\n private readonly float[] _value;\r\n private int _head; // index of the next write\r\n private int _count;\r\n\r\n /// <summary>Length of the trailing window in the same time unit passed to <see cref=\"Push\"/>.</summary>\r\n public float WindowSeconds { get; set; }\r\n\r\n public int Capacity => _time.Length;\r\n public bool HasSamples => _count > 0;\r\n\r\n public PlantWindow(float windowSeconds, int capacity = 512)\r\n {\r\n if (capacity < 1)\r\n capacity = 1;\r\n WindowSeconds = windowSeconds;\r\n _time = new float[capacity];\r\n _value = new float[capacity];\r\n }\r\n\r\n /// <summary>Discard all samples (e.g. on a hard clip cut) so the window rebuilds from scratch.</summary>\r\n public void Reset()\r\n {\r\n _head = 0;\r\n _count = 0;\r\n }\r\n\r\n /// <summary>\r\n /// Record a sample and return the trailing minimum over the window ending at\r\n /// <paramref name=\"now\"/>. With an empty history this is just <paramref name=\"value\"/>.\r\n /// </summary>\r\n public float Push(float now, float value)\r\n {\r\n _time[_head] = now;\r\n _value[_head] = value;\r\n _head = (_head + 1) % _time.Length;\r\n if (_count < _time.Length)\r\n _count++;\r\n\r\n return MinSince(now);\r\n }\r\n\r\n /// <summary>Trailing minimum over the window ending at <paramref name=\"now\"/> without adding a sample.</summary>\r\n public float MinSince(float now)\r\n {\r\n if (_count == 0)\r\n return 0f;\r\n\r\n float window = WindowSeconds;\r\n if (window < 0f)\r\n window = 0f;\r\n float cutoff = now - window;\r\n\r\n float min = float.MaxValue;\r\n // Walk from the most recent sample backward. Time is non-decreasing in push order, so\r\n // the first sample older than the cutoff means every earlier one is out of window too.\r\n for (int i = 0; i < _count; i++)\r\n {\r\n int idx = _head - 1 - i;\r\n if (idx < 0)\r\n idx += _time.Length;\r\n if (_time[idx] < cutoff)\r\n break;\r\n if (_value[idx] < min)\r\n min = _value[idx];\r\n }\r\n // At least the newest sample is always within [cutoff, now] when window >= 0.\r\n return min == float.MaxValue ? _value[(_head - 1 + _time.Length) % _time.Length] : min;\r\n }\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/TwoBoneIK.cs",
"FileName": "TwoBoneIK.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nusing System.Diagnostics.CodeAnalysis;\r\nusing Sandbox;\r\nusing BetterIk.Maths;\r\nusing BetterIk.Skeleton;\r\n\r\nnamespace BetterIk;\r\n\r\n/// <summary>\r\n/// Runtime two-bone IK (arm/leg) with pole vector control. Drop on the model root (or any\r\n/// descendant of it - the component finds the SkinnedModelRenderer via GetComponentInParent),\r\n/// set EndBone to the hand/foot bone name, optionally drag in a Target and PoleTarget. Walks\r\n/// up the skeleton automatically to find the mid and root bones; no per-model setup needed.\r\n/// </summary>\r\npublic sealed class TwoBoneIK : Component, IHasSkinnedRenderer\r\n{\r\n\t// --- Setup (the happy path: add component, set EndBone, drag Target, done) ---\r\n\t[Property] public SkinnedModelRenderer? Renderer { get; set; }\r\n\t[Property, BoneName] public string EndBone { get; set; } = \"\";\r\n\t[Property] public GameObject? Target { get; set; }\r\n\t[Property] public GameObject? PoleTarget { get; set; }\r\n\r\n\t// --- Weights (all lerp-safe every frame) ---\r\n\t[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;\r\n\t[Property, Range( 0f, 1f )] public float PositionWeight { get; set; } = 1f;\r\n\t[Property, Range( 0f, 1f )] public float RotationWeight { get; set; } = 1f;\r\n\r\n\t// --- Advanced: manual bone overrides, pole fine-tuning, soft/stretch (off by default) ---\r\n\t[Property, BoneName, Group( \"Advanced\" )] public string RootBoneOverride { get; set; } = \"\";\r\n\t[Property, BoneName, Group( \"Advanced\" )] public string MidBoneOverride { get; set; } = \"\";\r\n\t[Property, Group( \"Advanced\" ), Range( -180f, 180f )] public float PoleAngleOffsetDegrees { get; set; } = 0f;\r\n\t[Property, Group( \"Advanced\" ), Range( 0f, 0.49f )] public float SoftFraction { get; set; } = 0f;\r\n\t[Property, Group( \"Advanced\" ), Range( 0f, 1f )] public float MaxStretch { get; set; } = 0f;\r\n\r\n\t// --- Diagnostics ---\r\n\tpublic bool HasValidChain { get; private set; }\r\n\tpublic bool LastSolveApplied { get; private set; }\r\n\r\n\tprivate (SkinnedModelRenderer Renderer, string EndBone, string RootOverride, string MidOverride) _cachedSignature;\r\n\t// Only valid once EnsureResolved() has returned true at least once, same as a RequireComponent\r\n\t// field is only valid once OnStart has run.\r\n\tprivate BoneCollection.Bone _rootBone = null!;\r\n\tprivate BoneCollection.Bone _midBone = null!;\r\n\tprivate BoneCollection.Bone _endBone = null!;\r\n\tprivate BindPoseData _bindPose;\r\n\tprivate Vector3 _lastPoleDirection = Vector3.Up;\r\n\r\n\tprotected override void OnStart()\r\n\t{\r\n\t\tRenderer ??= GameObject.GetComponent<SkinnedModelRenderer>()\r\n\t\t\t?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );\r\n\t}\r\n\r\n\tprotected override void OnPreRender()\r\n\t{\r\n\t\tSolve();\r\n\t}\r\n\r\n\tprivate void Solve()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tHasValidChain = false;\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tHasValidChain = true;\r\n\r\n\t\tif ( Target is null || !Target.IsValid )\r\n\t\t{\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\t// Weight <= 0 skips the read-solve-write cycle entirely rather than reading the animated\r\n\t\t// pose and writing an \"unchanged\" result back. The world<->model-local round trip through\r\n\t\t// SetBoneTransform is NOT perfectly lossless every frame - on a rig with no real animation\r\n\t\t// driving it, this compounded into Infinity within a few seconds even though the math is\r\n\t\t// a no-op at Weight=0. Not writing at all when there is nothing to blend removes the\r\n\t\t// feedback path entirely.\r\n\t\tif ( Weight <= 0f )\r\n\t\t{\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rootBone, out var rootTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _midBone, out var midTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _endBone, out var endTx );\r\n\r\n\t\tVector3 poleHint = PoleTarget is not null && PoleTarget.IsValid\r\n\t\t\t? PoleTarget.WorldPosition - rootTx.Position\r\n\t\t\t: rootTx.Rotation * _bindPose.DefaultPoleDirection.ToSandbox();\r\n\r\n\t\t_lastPoleDirection = poleHint.Normal;\r\n\r\n\t\tvar input = new TwoBoneIkInput\r\n\t\t{\r\n\t\t\tRootPosition = rootTx.Position.ToNumerics(),\r\n\t\t\tMidPosition = midTx.Position.ToNumerics(),\r\n\t\t\tEndPosition = endTx.Position.ToNumerics(),\r\n\t\t\tRootRotation = rootTx.Rotation.ToNumerics(),\r\n\t\t\tMidRotation = midTx.Rotation.ToNumerics(),\r\n\t\t\tEndRotation = endTx.Rotation.ToNumerics(),\r\n\t\t\tTargetPosition = Target.WorldPosition.ToNumerics(),\r\n\t\t\tTargetRotation = Target.WorldRotation.ToNumerics(),\r\n\t\t\tHasPole = true, // poleHint always carries either the real pole or the bind-pose default\r\n\t\t\tPoleHint = poleHint.ToNumerics(),\r\n\t\t\tPoleAngleOffsetRadians = PoleAngleOffsetDegrees * (MathF.PI / 180f),\r\n\t\t\tFallbackBendNormal = _bindPose.BendNormal,\r\n\t\t\tPositionWeight = PositionWeight,\r\n\t\t\tRotationWeight = RotationWeight,\r\n\t\t\tMasterWeight = Weight,\r\n\t\t\tSoftFraction = SoftFraction,\r\n\t\t\tMaxStretch = MaxStretch,\r\n\t\t};\r\n\r\n\t\tvar result = TwoBoneIkSolver.Solve( input );\r\n\r\n\t\t// SetBoneTransform expects model-local space, unlike TryGetBoneTransformAnimation/\r\n\t\t// TryGetBoneTransform which are documented as worldspace - see MathBridge.ToModelLocal.\r\n\t\tRenderer.SetBoneTransform( in _rootBone, Renderer.ToModelLocal( new global::Transform( rootTx.Position, result.RootRotation.ToSandbox() ).WithScale( rootTx.Scale ) ) );\r\n\t\tRenderer.SetBoneTransform( in _midBone, Renderer.ToModelLocal( new global::Transform( result.MidPosition.ToSandbox(), result.MidRotation.ToSandbox() ).WithScale( midTx.Scale ) ) );\r\n\t\tRenderer.SetBoneTransform( in _endBone, Renderer.ToModelLocal( new global::Transform( result.EndPosition.ToSandbox(), result.EndRotation.ToSandbox() ).WithScale( endTx.Scale ) ) );\r\n\r\n\t\t// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML\r\n\t\t// docs; kept defensively until checklist item 3 is verified against a live editor.\r\n#pragma warning disable CS0612\r\n\t\tRenderer.PostAnimationUpdate();\r\n#pragma warning restore CS0612\r\n\r\n\t\tLastSolveApplied = result.Solved;\r\n\t}\r\n\r\n\t[MemberNotNullWhen( true, nameof( Renderer ) )]\r\n\tprivate bool EnsureResolved()\r\n\t{\r\n\t\tif ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( EndBone ) )\r\n\t\t\treturn false;\r\n\r\n\t\tvar signature = (Renderer, EndBone, RootBoneOverride, MidBoneOverride);\r\n\t\tif ( signature.Equals( _cachedSignature ) && _rootBone is not null )\r\n\t\t\treturn true;\r\n\r\n\t\tvar bones = Renderer.Model.Bones;\r\n\t\tif ( !bones.HasBone( EndBone ) )\r\n\t\t\treturn false;\r\n\r\n\t\tIBoneNode endNode = new SandboxBoneNode( bones.GetBone( EndBone ) );\r\n\r\n\t\tIBoneNode? rootOverrideNode = null;\r\n\t\tif ( !string.IsNullOrEmpty( RootBoneOverride ) )\r\n\t\t{\r\n\t\t\tif ( !bones.HasBone( RootBoneOverride ) )\r\n\t\t\t\treturn false;\r\n\t\t\trootOverrideNode = new SandboxBoneNode( bones.GetBone( RootBoneOverride ) );\r\n\t\t}\r\n\r\n\t\tIBoneNode? midOverrideNode = null;\r\n\t\tif ( !string.IsNullOrEmpty( MidBoneOverride ) )\r\n\t\t{\r\n\t\t\tif ( !bones.HasBone( MidBoneOverride ) )\r\n\t\t\t\treturn false;\r\n\t\t\tmidOverrideNode = new SandboxBoneNode( bones.GetBone( MidBoneOverride ) );\r\n\t\t}\r\n\r\n\t\tvar chain = BoneChainResolver.Resolve( endNode, rootOverrideNode, midOverrideNode );\r\n\t\tif ( !chain.Success )\r\n\t\t\treturn false;\r\n\r\n\t\t_rootBone = ((SandboxBoneNode)chain.Root!).Bone;\r\n\t\t_midBone = ((SandboxBoneNode)chain.Mid!).Bone;\r\n\t\t_endBone = ((SandboxBoneNode)chain.End!).Bone;\r\n\t\t_cachedSignature = signature;\r\n\r\n\t\t// Bind-pose world positions, composed from an arbitrary origin at the root - only the\r\n\t\t// relative offsets matter to AnalyzeBindPose (translation-invariant), so we don't need\r\n\t\t// to walk all the way up to the model's true origin.\r\n\t\tvar rootBindWorld = global::Transform.Zero;\r\n\t\tvar midBindWorld = global::Transform.Concat( rootBindWorld, _midBone.LocalTransform );\r\n\t\tvar endBindWorld = global::Transform.Concat( midBindWorld, _endBone.LocalTransform );\r\n\r\n\t\t_bindPose = TwoBoneIkSolver.AnalyzeBindPose(\r\n\t\t\trootBindWorld.Position.ToNumerics(),\r\n\t\t\tmidBindWorld.Position.ToNumerics(),\r\n\t\t\tendBindWorld.Position.ToNumerics() );\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\tprotected override void DrawGizmos()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tGizmo.Draw.Color = Color.Red;\r\n\t\t\tGizmo.Draw.WorldText( \"TwoBoneIK: invalid bone chain\", new global::Transform( WorldPosition ) );\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tif ( Target is null || !Target.IsValid )\r\n\t\t\treturn;\r\n\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rootBone, out var rootTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _midBone, out var midTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _endBone, out var endTx );\r\n\r\n\t\tfloat l1 = (midTx.Position - rootTx.Position).Length;\r\n\t\tfloat l2 = (endTx.Position - midTx.Position).Length;\r\n\t\tfloat lmax = l1 + l2;\r\n\t\tfloat d = (Target.WorldPosition - rootTx.Position).Length;\r\n\t\tfloat minReach = MathF.Abs( l1 - l2 );\r\n\t\tfloat softStart = (1f - SoftFraction) * lmax;\r\n\r\n\t\tColor reachColor;\r\n\t\tif ( d < minReach || d > lmax * (1f + MaxStretch) )\r\n\t\t\treachColor = Color.Red;\r\n\t\telse if ( SoftFraction > 0f && d > softStart )\r\n\t\t\treachColor = Color.Yellow;\r\n\t\telse\r\n\t\t\treachColor = Color.Green;\r\n\r\n\t\t// Solved chain (post-IK), reusing the same read this frame's Solve() already wrote.\r\n\t\tRenderer.TryGetBoneTransform( in _rootBone, out var solvedRoot );\r\n\t\tRenderer.TryGetBoneTransform( in _midBone, out var solvedMid );\r\n\t\tRenderer.TryGetBoneTransform( in _endBone, out var solvedEnd );\r\n\r\n\t\tGizmo.Draw.Color = reachColor;\r\n\t\tGizmo.Draw.Line( solvedRoot.Position, solvedMid.Position );\r\n\t\tGizmo.Draw.Line( solvedMid.Position, solvedEnd.Position );\r\n\r\n\t\tGizmo.Draw.Color = Color.White;\r\n\t\tGizmo.Draw.LineSphere( new Sphere( Target.WorldPosition, MathF.Max( lmax * 0.05f, 1f ) ) );\r\n\r\n\t\tGizmo.Draw.Color = PoleTarget is not null && PoleTarget.IsValid ? Color.White : Color.Cyan;\r\n\t\tGizmo.Draw.Arrow( solvedMid.Position, solvedMid.Position + _lastPoleDirection * (lmax * 0.3f), lmax * 0.05f, lmax * 0.03f );\r\n\r\n\t\tGizmo.Draw.Color = Color.Cyan.WithAlpha( 0.15f );\r\n\t\tGizmo.Draw.SolidTriangle( new Triangle { A = solvedRoot.Position, B = solvedMid.Position, C = solvedEnd.Position } );\r\n\t}\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/Maths/IkMath.cs",
"FileName": "IkMath.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\nusing Vector3 = System.Numerics.Vector3;\r\nusing Quaternion = System.Numerics.Quaternion;\r\nusing Matrix4x4 = System.Numerics.Matrix4x4;\r\n\r\ninternal static class IkMath\r\n{\r\n public static Vector3 SafeNormalize(Vector3 v, Vector3 fallback)\r\n {\r\n float lenSq = v.LengthSquared();\r\n if (lenSq < 1e-12f)\r\n return fallback;\r\n return v / MathF.Sqrt(lenSq);\r\n }\r\n\r\n public static Vector3 ProjectPerpendicular(Vector3 v, Vector3 unitAxis)\r\n {\r\n return v - Vector3.Dot(v, unitAxis) * unitAxis;\r\n }\r\n\r\n // Deterministic arbitrary perpendicular: cross with whichever world axis has the\r\n // smallest component along unitAxis, so the result never collapses to zero.\r\n public static Vector3 AnyPerpendicular(Vector3 unitAxis)\r\n {\r\n float ax = MathF.Abs(unitAxis.X);\r\n float ay = MathF.Abs(unitAxis.Y);\r\n float az = MathF.Abs(unitAxis.Z);\r\n\r\n Vector3 seed = (ax <= ay && ax <= az) ? Vector3.UnitX\r\n : (ay <= az) ? Vector3.UnitY\r\n : Vector3.UnitZ;\r\n\r\n Vector3 perp = Vector3.Cross(unitAxis, seed);\r\n if (perp.LengthSquared() < 1e-12f)\r\n {\r\n seed = seed == Vector3.UnitX ? Vector3.UnitY : Vector3.UnitX;\r\n perp = Vector3.Cross(unitAxis, seed);\r\n }\r\n\r\n return Vector3.Normalize(perp);\r\n }\r\n\r\n // U = dir, W = Gram-Schmidt of normalHint against U (fallback to AnyPerpendicular if degenerate), V = U x W.\r\n public static (Vector3 U, Vector3 W, Vector3 V) BuildOrthonormalBasis(Vector3 dir, Vector3 normalHint)\r\n {\r\n Vector3 u = SafeNormalize(dir, Vector3.UnitX);\r\n Vector3 wRaw = ProjectPerpendicular(normalHint, u);\r\n Vector3 w = wRaw.LengthSquared() < 1e-10f ? AnyPerpendicular(u) : Vector3.Normalize(wRaw);\r\n Vector3 v = Vector3.Cross(u, w);\r\n return (u, w, v);\r\n }\r\n\r\n // Rotation mapping the old (dir, normalHint) frame exactly onto the new (dir, normalHint) frame:\r\n // delta * oldU = newU, delta * oldW = newW, delta * oldV = newV. Built via change-of-basis matrices\r\n // rather than shortest-arc, so twist/roll is pinned and there is no 180-degree flip ambiguity.\r\n public static Quaternion DeltaRotation(Vector3 oldDir, Vector3 oldNormalHint, Vector3 newDir, Vector3 newNormalHint)\r\n {\r\n var (uOld, wOld, vOld) = BuildOrthonormalBasis(oldDir, oldNormalHint);\r\n var (uNew, wNew, vNew) = BuildOrthonormalBasis(newDir, newNormalHint);\r\n\r\n // Row-vector convention (matches System.Numerics Vector3.Transform(v, Matrix4x4)):\r\n // row i of the matrix is where local axis i maps to.\r\n var mOld = new Matrix4x4(\r\n uOld.X, uOld.Y, uOld.Z, 0f,\r\n wOld.X, wOld.Y, wOld.Z, 0f,\r\n vOld.X, vOld.Y, vOld.Z, 0f,\r\n 0f, 0f, 0f, 1f);\r\n\r\n var mNew = new Matrix4x4(\r\n uNew.X, uNew.Y, uNew.Z, 0f,\r\n wNew.X, wNew.Y, wNew.Z, 0f,\r\n vNew.X, vNew.Y, vNew.Z, 0f,\r\n 0f, 0f, 0f, 1f);\r\n\r\n // mOld is orthonormal, so its inverse is its transpose: delta = mOld^-1 * mNew.\r\n var delta = Matrix4x4.Transpose(mOld) * mNew;\r\n return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(delta));\r\n }\r\n\r\n // Shortest-arc rotation mapping unit vector `from` onto unit vector `to`. No twist/roll control\r\n // around the resulting axis (there is no well-defined \"roll\" for a pure vector-to-vector map).\r\n internal static Quaternion FromToRotation(Vector3 from, Vector3 to)\r\n {\r\n float cosAngle = Math.Clamp(Vector3.Dot(from, to), -1f, 1f);\r\n\r\n if (cosAngle > 1f - 1e-7f)\r\n return Quaternion.Identity;\r\n\r\n if (cosAngle < -1f + 1e-7f)\r\n {\r\n Vector3 axis180 = AnyPerpendicular(from);\r\n return Quaternion.CreateFromAxisAngle(axis180, MathF.PI);\r\n }\r\n\r\n Vector3 axis = Vector3.Normalize(Vector3.Cross(from, to));\r\n float angle = MathF.Acos(cosAngle);\r\n return Quaternion.CreateFromAxisAngle(axis, angle);\r\n }\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/Skeleton/BoneChainResult.cs",
"FileName": "BoneChainResult.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nnamespace BetterIk.Skeleton;\r\n\r\npublic readonly struct BoneChainResult\r\n{\r\n public readonly bool Success;\r\n public readonly BoneChainError Error;\r\n public readonly IBoneNode? Root;\r\n public readonly IBoneNode? Mid;\r\n public readonly IBoneNode? End;\r\n\r\n private BoneChainResult(bool success, BoneChainError error, IBoneNode? root, IBoneNode? mid, IBoneNode? end)\r\n {\r\n Success = success;\r\n Error = error;\r\n Root = root;\r\n Mid = mid;\r\n End = end;\r\n }\r\n\r\n public static BoneChainResult Ok(IBoneNode root, IBoneNode mid, IBoneNode end)\r\n => new(true, BoneChainError.None, root, mid, end);\r\n\r\n public static BoneChainResult Fail(BoneChainError error)\r\n => new(false, error, null, null, null);\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/FootPlacementIK.cs",
"FileName": "FootPlacementIK.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nusing System;\r\nusing System.Diagnostics.CodeAnalysis;\r\nusing Sandbox;\r\nusing BetterIk.Maths;\r\nusing BetterIk.Skeleton;\r\n\r\nnamespace BetterIk;\r\n\r\n/// <summary>\r\n/// Foot grounding: traces down under each foot, plants feet on real geometry, drops the pelvis\r\n/// when the ground is lower so the far foot can reach, and aligns foot rotation to the surface\r\n/// normal. Drop on the model root (or any descendant), set LeftFootBone/RightFootBone to the\r\n/// ankle/foot bone names. Root and mid bones for each leg are auto-walked like TwoBoneIK; the\r\n/// pelvis is auto-derived as the nearest common ancestor of the two resolved leg roots.\r\n/// </summary>\r\npublic sealed class FootPlacementIK : Component, IHasSkinnedRenderer\r\n{\r\n\t[Property] public SkinnedModelRenderer? Renderer { get; set; }\r\n\t[Property, BoneName] public string LeftFootBone { get; set; } = \"\";\r\n\t[Property, BoneName] public string RightFootBone { get; set; } = \"\";\r\n\r\n\t[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;\r\n\t[Property, Range( 0f, 1f )] public float FootRotationWeight { get; set; } = 1f;\r\n\r\n\t[Property, Group( \"Ground\" )] public float MaxStepUp { get; set; } = 18f;\r\n\t[Property, Group( \"Ground\" )] public float MaxStepDown { get; set; } = 18f;\r\n\t[Property, Group( \"Ground\" )] public float MaxPelvisDrop { get; set; } = 16f;\r\n\t[Property, Group( \"Ground\" )] public float MaxPelvisRaise { get; set; } = 0f;\r\n\t[Property, Group( \"Ground\" ), Range( 0f, 90f )] public float MaxFootRotationDegrees { get; set; } = 30f;\r\n\t[Property, Group( \"Ground\" ), Range( 0f, 90f )] public float MaxGroundSlopeDegrees { get; set; } = 60f;\r\n\t[Property, Group( \"Ground\" )] public float FootHeightOffset { get; set; } = 0f;\r\n\t[Property, Group( \"Ground\" )] public float SmoothingRate { get; set; } = 10f;\r\n\t[Property, Group( \"Ground\" )] public string IgnoreTags { get; set; } = \"player\";\r\n\r\n\t// ---- Plant-to-ground mode (opt-in) -----------------------------------------------------\r\n\t// The default (terrain) mode preserves the animation's authored height above an assumed\r\n\t// flat ground and only adapts to terrain deviation, which is correct for grounded\r\n\t// animations. Plant mode instead corrects animations whose feet HOVER above the ground\r\n\t// they were authored to touch (a per-clip constant conversion offset, possibly different\r\n\t// per foot): each foot's plant point (an optional plant bone, e.g. the ball of the foot;\r\n\t// the foot bone itself when unset) is measured against the ground, its TRAILING-MINIMUM\r\n\t// height over PlantWindowSeconds is treated as the hover to remove, and the foot is\r\n\t// lowered vertically by that amount with its AUTHORED ROTATION UNTOUCHED. Removing only\r\n\t// the trailing minimum keeps every within-window articulation intact: authored step\r\n\t// lifts and heel raises ride on top of the corrected plant level, flat-authored feet\r\n\t// come out flat, and nothing is ever rotated onto the surface. Pelvis is not moved in\r\n\t// plant mode (correct a shared full-body offset at the model root instead).\r\n\t[Property, Group( \"Plant\" )] public bool PlantToGround { get; set; } = false;\r\n\t[Property, BoneName, Group( \"Plant\" )] public string LeftPlantBone { get; set; } = \"\";\r\n\t[Property, BoneName, Group( \"Plant\" )] public string RightPlantBone { get; set; } = \"\";\r\n\t/// <summary>Rest height of the plant point above the ground when planted (its bind height).</summary>\r\n\t[Property, Group( \"Plant\" )] public float PlantHeightOffset { get; set; } = 0f;\r\n\t/// <summary>Trailing window (seconds) whose minimum plant-point height is removed as hover.</summary>\r\n\t[Property, Group( \"Plant\" )] public float PlantWindowSeconds { get; set; } = 0.8f;\r\n\t/// <summary>Upper clamp on the per-foot plant correction.</summary>\r\n\t[Property, Group( \"Plant\" )] public float PlantMaxCorrection { get; set; } = 6f;\r\n\t/// <summary>Skip tracing and use a flat world-space ground plane at FlatGroundHeight.</summary>\r\n\t[Property, Group( \"Plant\" )] public bool UseFlatGround { get; set; } = false;\r\n\t[Property, Group( \"Plant\" )] public float FlatGroundHeight { get; set; } = 0f;\r\n\r\n\t/// <summary>Live plant corrections (world units, positive = foot lowered), for diagnostics.</summary>\r\n\tpublic float CurrentPlantCorrectionL { get; private set; }\r\n\tpublic float CurrentPlantCorrectionR { get; private set; }\r\n\t/// <summary>Live plant-point heights above the ground before correction, for diagnostics.</summary>\r\n\tpublic float LastPlantResidualL { get; private set; }\r\n\tpublic float LastPlantResidualR { get; private set; }\r\n\r\n\t[Property, BoneName, Group( \"Advanced\" )] public string PelvisBoneOverride { get; set; } = \"\";\r\n\t[Property, Group( \"Advanced\" )] public GameObject? LeftPoleTarget { get; set; }\r\n\t[Property, Group( \"Advanced\" )] public GameObject? RightPoleTarget { get; set; }\r\n\t[Property, BoneName, Group( \"Advanced\" )] public string LeftRootOverride { get; set; } = \"\";\r\n\t[Property, BoneName, Group( \"Advanced\" )] public string LeftMidOverride { get; set; } = \"\";\r\n\t[Property, BoneName, Group( \"Advanced\" )] public string RightRootOverride { get; set; } = \"\";\r\n\t[Property, BoneName, Group( \"Advanced\" )] public string RightMidOverride { get; set; } = \"\";\r\n\r\n\tpublic bool HasValidChains { get; private set; }\r\n\tpublic bool PelvisResolved { get; private set; }\r\n\tpublic bool LeftGrounded { get; private set; }\r\n\tpublic bool RightGrounded { get; private set; }\r\n\tpublic float CurrentPelvisOffset { get; private set; }\r\n\r\n\tprivate (SkinnedModelRenderer Renderer, string LeftFootBone, string RightFootBone, string LeftRootOverride, string LeftMidOverride, string RightRootOverride, string RightMidOverride, string PelvisBoneOverride, string LeftPlantBone, string RightPlantBone) _cachedSignature;\r\n\r\n\t// Only valid once EnsureResolved() has returned true at least once.\r\n\tprivate BoneCollection.Bone _leftRoot = null!;\r\n\tprivate BoneCollection.Bone _leftMid = null!;\r\n\tprivate BoneCollection.Bone _leftEnd = null!;\r\n\tprivate BoneCollection.Bone _rightRoot = null!;\r\n\tprivate BoneCollection.Bone _rightMid = null!;\r\n\tprivate BoneCollection.Bone _rightEnd = null!;\r\n\tprivate BoneCollection.Bone _pelvisBone = null!;\r\n\r\n\t// Plant points (plant mode): the ball / toe bone whose ground contact is planted. Falls\r\n\t// back to the leg's own end bone when the name is unset or not present on the model.\r\n\tprivate BoneCollection.Bone _leftPlant = null!;\r\n\tprivate BoneCollection.Bone _rightPlant = null!;\r\n\tprivate bool _leftPlantResolved;\r\n\tprivate bool _rightPlantResolved;\r\n\r\n\tprivate BindPoseData _leftBindPose;\r\n\tprivate BindPoseData _rightBindPose;\r\n\r\n\tprivate float _smoothPelvis;\r\n\tprivate float _smoothDeltaL;\r\n\tprivate float _smoothDeltaR;\r\n\r\n\t// Plant mode state: trailing-minimum windows over each plant point's height above ground,\r\n\t// and the smoothed per-foot downward correction actually applied.\r\n\tprivate PlantWindow _plantWindowL;\r\n\tprivate PlantWindow _plantWindowR;\r\n\tprivate float _smoothPlantL;\r\n\tprivate float _smoothPlantR;\r\n\r\n\tprotected override void OnStart()\r\n\t{\r\n\t\tRenderer ??= GameObject.GetComponent<SkinnedModelRenderer>()\r\n\t\t\t?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );\r\n\t}\r\n\r\n\tprotected override void OnPreRender()\r\n\t{\r\n\t\tSolve();\r\n\t}\r\n\r\n\tprivate void Solve()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tHasValidChains = false;\r\n\t\t\tLeftGrounded = false;\r\n\t\t\tRightGrounded = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tHasValidChains = true;\r\n\r\n\t\t// Weight <= 0 skips the entire read-trace-solve-write cycle: a SetBoneTransform whose\r\n\t\t// value has no restoring force (not animated, not converging toward a fixed target) is\r\n\t\t// not bit-lossless at the engine write boundary and compounds to Infinity over enough\r\n\t\t// frames (confirmed via FabrikIK). Zero the smoothed state so re-enabling Weight later\r\n\t\t// doesn't replay stale offsets.\r\n\t\tif ( Weight <= 0f )\r\n\t\t{\r\n\t\t\t_smoothPelvis = 0f;\r\n\t\t\t_smoothDeltaL = 0f;\r\n\t\t\t_smoothDeltaR = 0f;\r\n\t\t\t_smoothPlantL = 0f;\r\n\t\t\t_smoothPlantR = 0f;\r\n\t\t\t_plantWindowL?.Reset();\r\n\t\t\t_plantWindowR?.Reset();\r\n\t\t\tLeftGrounded = false;\r\n\t\t\tRightGrounded = false;\r\n\t\t\tCurrentPelvisOffset = 0f;\r\n\t\t\tCurrentPlantCorrectionL = 0f;\r\n\t\t\tCurrentPlantCorrectionR = 0f;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tif ( PlantToGround )\r\n\t\t{\r\n\t\t\tSolvePlant();\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _leftRoot, out var leftRootTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _leftMid, out var leftMidTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _leftEnd, out var leftEndTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rightRoot, out var rightRootTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rightMid, out var rightMidTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rightEnd, out var rightEndTx );\r\n\r\n\t\tvar pelvisTx = global::Transform.Zero;\r\n\t\tif ( PelvisResolved )\r\n\t\t\tRenderer.TryGetBoneTransformAnimation( in _pelvisBone, out pelvisTx );\r\n\r\n\t\tVector3 up = Vector3.Up;\r\n\t\tVector3 origin = Renderer.WorldPosition;\r\n\r\n\t\tvar leftTrace = TraceFoot( leftEndTx.Position, up, origin );\r\n\t\tvar rightTrace = TraceFoot( rightEndTx.Position, up, origin );\r\n\r\n\t\tvar input = new FootPlacementInput\r\n\t\t{\r\n\t\t\tLeftFoot = new FootInput\r\n\t\t\t{\r\n\t\t\t\tFootPosition = leftEndTx.Position.ToNumerics(),\r\n\t\t\t\tFootRotation = leftEndTx.Rotation.ToNumerics(),\r\n\t\t\t\tHasHit = leftTrace.Hit,\r\n\t\t\t\tHitPoint = leftTrace.HitPosition.ToNumerics(),\r\n\t\t\t\tHitNormal = leftTrace.Normal.ToNumerics(),\r\n\t\t\t},\r\n\t\t\tRightFoot = new FootInput\r\n\t\t\t{\r\n\t\t\t\tFootPosition = rightEndTx.Position.ToNumerics(),\r\n\t\t\t\tFootRotation = rightEndTx.Rotation.ToNumerics(),\r\n\t\t\t\tHasHit = rightTrace.Hit,\r\n\t\t\t\tHitPoint = rightTrace.HitPosition.ToNumerics(),\r\n\t\t\t\tHitNormal = rightTrace.Normal.ToNumerics(),\r\n\t\t\t},\r\n\t\t\tOriginPosition = origin.ToNumerics(),\r\n\t\t\tUpAxis = up.ToNumerics(),\r\n\t\t\tFootHeightOffset = FootHeightOffset,\r\n\t\t\tMaxStepUp = MaxStepUp,\r\n\t\t\tMaxStepDown = MaxStepDown,\r\n\t\t\tMaxPelvisDrop = MaxPelvisDrop,\r\n\t\t\tMaxPelvisRaise = MaxPelvisRaise,\r\n\t\t\tMaxFootRotationRadians = MaxFootRotationDegrees * (MathF.PI / 180f),\r\n\t\t\tMaxGroundSlopeRadians = MaxGroundSlopeDegrees * (MathF.PI / 180f),\r\n\t\t\tWeight = Weight,\r\n\t\t};\r\n\r\n\t\tvar result = FootPlacementSolver.Solve( input );\r\n\r\n\t\tfloat dt = Time.Delta;\r\n\t\t_smoothPelvis = FootPlacementSolver.SmoothOffset( _smoothPelvis, result.PelvisOffset, SmoothingRate, dt );\r\n\t\t_smoothDeltaL = FootPlacementSolver.SmoothOffset( _smoothDeltaL, result.LeftFoot.VerticalDelta, SmoothingRate, dt );\r\n\t\t_smoothDeltaR = FootPlacementSolver.SmoothOffset( _smoothDeltaR, result.RightFoot.VerticalDelta, SmoothingRate, dt );\r\n\r\n\t\t// Snap to exact zero once within epsilon of a zero target: exponential decay (SmoothOffset)\r\n\t\t// never reaches exact 0 on its own, which would otherwise keep the hazardous \"write back\r\n\t\t// an unchanged value forever\" pattern alive permanently on flat ground / an ungrounded foot.\r\n\t\tif ( result.PelvisOffset == 0f && MathF.Abs( _smoothPelvis ) < 1e-3f )\r\n\t\t\t_smoothPelvis = 0f;\r\n\t\tif ( result.LeftFoot.VerticalDelta == 0f && MathF.Abs( _smoothDeltaL ) < 1e-3f )\r\n\t\t\t_smoothDeltaL = 0f;\r\n\t\tif ( result.RightFoot.VerticalDelta == 0f && MathF.Abs( _smoothDeltaR ) < 1e-3f )\r\n\t\t\t_smoothDeltaR = 0f;\r\n\r\n\t\tLeftGrounded = result.LeftFoot.Grounded;\r\n\t\tRightGrounded = result.RightFoot.Grounded;\r\n\t\tCurrentPelvisOffset = _smoothPelvis;\r\n\r\n\t\t// Skip the pelvis write entirely once its offset has snapped to exact zero - a zero-offset\r\n\t\t// write targets exactly the animated pose anyway, so skipping it is not a behavior change,\r\n\t\t// only a removal of an unforced no-op write (see the principle above).\r\n\t\tif ( PelvisResolved && _smoothPelvis != 0f )\r\n\t\t{\r\n\t\t\t// SetBoneTransform expects model-local space - see MathBridge.ToModelLocal.\r\n\t\t\tRenderer.SetBoneTransform( in _pelvisBone, Renderer.ToModelLocal( new global::Transform( pelvisTx.Position + up * _smoothPelvis, pelvisTx.Rotation ).WithScale( pelvisTx.Scale ) ) );\r\n\t\t}\r\n\r\n\t\tVector3 pelvisShift = up * (PelvisResolved ? _smoothPelvis : 0f);\r\n\r\n\t\t// Skip a leg's writes only when there is genuinely nothing driving them: ungrounded (no\r\n\t\t// trace hit), its smoothed delta has snapped to zero, AND the pelvis isn't shifting it\r\n\t\t// either. A nonzero pelvis shift still requires the leg solve even for an otherwise-settled\r\n\t\t// ungrounded foot, since the leg still needs to follow the pelvis. Otherwise, an ungrounded\r\n\t\t// foot's TargetPosition is derived from this frame's own read (endTx.Position + up*0), which\r\n\t\t// has no restoring force once settled - the same unforced-write hazard as the pelvis case.\r\n\t\tbool leftNeedsWrite = result.LeftFoot.Grounded || _smoothDeltaL != 0f || pelvisShift != Vector3.Zero;\r\n\t\tbool rightNeedsWrite = result.RightFoot.Grounded || _smoothDeltaR != 0f || pelvisShift != Vector3.Zero;\r\n\r\n\t\tif ( leftNeedsWrite )\r\n\t\t\tSolveLeg( in _leftRoot, in _leftMid, in _leftEnd, leftRootTx, leftMidTx, leftEndTx, _leftBindPose, pelvisShift, _smoothDeltaL, result.LeftFoot.TargetRotation, LeftPoleTarget, up );\r\n\t\tif ( rightNeedsWrite )\r\n\t\t\tSolveLeg( in _rightRoot, in _rightMid, in _rightEnd, rightRootTx, rightMidTx, rightEndTx, _rightBindPose, pelvisShift, _smoothDeltaR, result.RightFoot.TargetRotation, RightPoleTarget, up );\r\n\r\n\t\t// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML\r\n\t\t// docs; kept defensively, matching TwoBoneIK/LookAtIK's already-verified usage.\r\n#pragma warning disable CS0612\r\n\t\tRenderer.PostAnimationUpdate();\r\n#pragma warning restore CS0612\r\n\t}\r\n\r\n\t// Plant mode (opt-in via PlantToGround): correct each foot's hover WITHOUT moving the pelvis.\r\n\t// Each foot's plant point (the ball bone, or the leg end when unset) is measured against the\r\n\t// ground; its trailing-minimum height over PlantWindowSeconds is the planted level, and the\r\n\t// foot is lowered by exactly that hover (down to PlantHeightOffset) with its authored rotation\r\n\t// kept. Authored step lifts and heel raises above the planted level ride on top untouched;\r\n\t// flat-authored feet come out flat; a grounded foot is never raised. The pelvis is left alone:\r\n\t// a shared full-body offset belongs on the model root, not here.\r\n\tprivate void SolvePlant()\r\n\t{\r\n\t\t_plantWindowL ??= new PlantWindow( PlantWindowSeconds );\r\n\t\t_plantWindowR ??= new PlantWindow( PlantWindowSeconds );\r\n\t\t_plantWindowL.WindowSeconds = PlantWindowSeconds;\r\n\t\t_plantWindowR.WindowSeconds = PlantWindowSeconds;\r\n\r\n\t\tVector3 up = Vector3.Up;\r\n\t\tVector3 origin = Renderer.WorldPosition;\r\n\t\tfloat now = Time.Now;\r\n\t\tfloat dt = Time.Delta;\r\n\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _leftRoot, out var leftRootTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _leftMid, out var leftMidTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _leftEnd, out var leftEndTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rightRoot, out var rightRootTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rightMid, out var rightMidTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rightEnd, out var rightEndTx );\r\n\r\n\t\tvar leftPlantBone = _leftPlantResolved ? _leftPlant : _leftEnd;\r\n\t\tvar rightPlantBone = _rightPlantResolved ? _rightPlant : _rightEnd;\r\n\t\tRenderer.TryGetBoneTransformAnimation( in leftPlantBone, out var leftPlantTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in rightPlantBone, out var rightPlantTx );\r\n\r\n\t\t_smoothPlantL = SolvePlantCorrection( leftPlantTx.Position, up, origin, now, dt, _plantWindowL, _smoothPlantL,\r\n\t\t\tout bool leftGrounded, out float leftResidual );\r\n\t\t_smoothPlantR = SolvePlantCorrection( rightPlantTx.Position, up, origin, now, dt, _plantWindowR, _smoothPlantR,\r\n\t\t\tout bool rightGrounded, out float rightResidual );\r\n\r\n\t\tLeftGrounded = leftGrounded;\r\n\t\tRightGrounded = rightGrounded;\r\n\t\tLastPlantResidualL = leftResidual;\r\n\t\tLastPlantResidualR = rightResidual;\r\n\t\tCurrentPlantCorrectionL = _smoothPlantL;\r\n\t\tCurrentPlantCorrectionR = _smoothPlantR;\r\n\t\tCurrentPelvisOffset = 0f;\r\n\r\n\t\t// A settled zero correction skips the leg write entirely (the target IS the animated pose,\r\n\t\t// so the write is an unforced no-op - the same hazard the terrain path guards against).\r\n\t\tif ( _smoothPlantL != 0f )\r\n\t\t\tSolveLeg( in _leftRoot, in _leftMid, in _leftEnd, leftRootTx, leftMidTx, leftEndTx, _leftBindPose,\r\n\t\t\t\tVector3.Zero, -_smoothPlantL, leftEndTx.Rotation.ToNumerics(), LeftPoleTarget, up );\r\n\t\tif ( _smoothPlantR != 0f )\r\n\t\t\tSolveLeg( in _rightRoot, in _rightMid, in _rightEnd, rightRootTx, rightMidTx, rightEndTx, _rightBindPose,\r\n\t\t\t\tVector3.Zero, -_smoothPlantR, rightEndTx.Rotation.ToNumerics(), RightPoleTarget, up );\r\n\r\n#pragma warning disable CS0612\r\n\t\tRenderer.PostAnimationUpdate();\r\n#pragma warning restore CS0612\r\n\t}\r\n\r\n\t// One foot's smoothed downward plant correction (world units, positive = foot lowered). Reads\r\n\t// the plant point against the ground (a flat plane or a downward trace), pushes its height into\r\n\t// the trailing-minimum window, removes the trailing-minimum hover down to PlantHeightOffset,\r\n\t// and smooths the result. Returns 0 when no ground reference is available (nothing to plant to).\r\n\tprivate float SolvePlantCorrection( Vector3 plantPos, Vector3 up, Vector3 origin, float now, float dt,\r\n\t\tPlantWindow window, float smoothPrev, out bool grounded, out float residual )\r\n\t{\r\n\t\tfloat groundLevel;\r\n\t\tif ( UseFlatGround )\r\n\t\t{\r\n\t\t\tgrounded = true;\r\n\t\t\tgroundLevel = FlatGroundHeight;\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tvar tr = TraceFoot( plantPos, up, origin );\r\n\t\t\tgrounded = tr.Hit;\r\n\t\t\tgroundLevel = tr.Hit ? Vector3.Dot( tr.HitPosition, up ) : 0f;\r\n\t\t}\r\n\r\n\t\t// Height of the plant point above the ground along the up axis (the hover to remove).\r\n\t\tresidual = Vector3.Dot( plantPos, up ) - groundLevel;\r\n\r\n\t\tfloat target = 0f;\r\n\t\tif ( grounded )\r\n\t\t{\r\n\t\t\tfloat trailingMin = window.Push( now, residual );\r\n\t\t\ttarget = PlantToGroundSolver.ComputeCorrection( trailingMin, PlantHeightOffset, PlantMaxCorrection );\r\n\t\t}\r\n\r\n\t\tfloat smoothed = FootPlacementSolver.SmoothOffset( smoothPrev, target, SmoothingRate, dt );\r\n\t\tif ( target == 0f && MathF.Abs( smoothed ) < 1e-3f )\r\n\t\t\tsmoothed = 0f;\r\n\t\treturn smoothed;\r\n\t}\r\n\r\n\tprivate void SolveLeg( in BoneCollection.Bone rootBone, in BoneCollection.Bone midBone, in BoneCollection.Bone endBone,\r\n\t\tglobal::Transform rootTx, global::Transform midTx, global::Transform endTx, BindPoseData bindPose,\r\n\t\tVector3 pelvisShift, float footDelta, System.Numerics.Quaternion footTargetRotation, GameObject? poleTarget, Vector3 up )\r\n\t{\r\n\t\tVector3 shiftedRoot = rootTx.Position + pelvisShift;\r\n\t\tVector3 shiftedMid = midTx.Position + pelvisShift;\r\n\t\tVector3 shiftedEnd = endTx.Position + pelvisShift;\r\n\r\n\t\tVector3 poleHint = poleTarget is not null && poleTarget.IsValid\r\n\t\t\t? poleTarget.WorldPosition - shiftedRoot\r\n\t\t\t: rootTx.Rotation * bindPose.DefaultPoleDirection.ToSandbox();\r\n\r\n\t\tvar input = new TwoBoneIkInput\r\n\t\t{\r\n\t\t\tRootPosition = shiftedRoot.ToNumerics(),\r\n\t\t\tMidPosition = shiftedMid.ToNumerics(),\r\n\t\t\tEndPosition = shiftedEnd.ToNumerics(),\r\n\t\t\tRootRotation = rootTx.Rotation.ToNumerics(),\r\n\t\t\tMidRotation = midTx.Rotation.ToNumerics(),\r\n\t\t\tEndRotation = endTx.Rotation.ToNumerics(),\r\n\t\t\tTargetPosition = (endTx.Position + up * footDelta).ToNumerics(),\r\n\t\t\tTargetRotation = footTargetRotation,\r\n\t\t\tHasPole = true,\r\n\t\t\tPoleHint = poleHint.ToNumerics(),\r\n\t\t\tPoleAngleOffsetRadians = 0f,\r\n\t\t\tFallbackBendNormal = bindPose.BendNormal,\r\n\t\t\tPositionWeight = 1f,\r\n\t\t\tRotationWeight = FootRotationWeight,\r\n\t\t\tMasterWeight = Weight,\r\n\t\t\tSoftFraction = 0f,\r\n\t\t\tMaxStretch = 0f,\r\n\t\t};\r\n\r\n\t\tvar result = TwoBoneIkSolver.Solve( input );\r\n\r\n\t\t// SetBoneTransform expects model-local space, unlike TryGetBoneTransformAnimation/\r\n\t\t// TryGetBoneTransform which are documented as worldspace - see MathBridge.ToModelLocal.\r\n\t\tRenderer!.SetBoneTransform( in rootBone, Renderer.ToModelLocal( new global::Transform( shiftedRoot, result.RootRotation.ToSandbox() ).WithScale( rootTx.Scale ) ) );\r\n\t\tRenderer.SetBoneTransform( in midBone, Renderer.ToModelLocal( new global::Transform( result.MidPosition.ToSandbox(), result.MidRotation.ToSandbox() ).WithScale( midTx.Scale ) ) );\r\n\t\tRenderer.SetBoneTransform( in endBone, Renderer.ToModelLocal( new global::Transform( result.EndPosition.ToSandbox(), result.EndRotation.ToSandbox() ).WithScale( endTx.Scale ) ) );\r\n\t}\r\n\r\n\tprivate (bool Hit, Vector3 HitPosition, Vector3 Normal) TraceFoot( Vector3 footPos, Vector3 up, Vector3 origin )\r\n\t{\r\n\t\tconst float traceMargin = 2f;\r\n\t\tVector3 footOnPlane = footPos - up * Vector3.Dot( footPos - origin, up );\r\n\t\tVector3 traceStart = footOnPlane + up * (MaxStepUp + traceMargin);\r\n\t\tVector3 traceEnd = footOnPlane - up * (MaxStepDown + traceMargin);\r\n\r\n\t\tvar trace = Scene.Trace.FromTo( traceStart, traceEnd ).IgnoreGameObjectHierarchy( Renderer!.GameObject.Root );\r\n\r\n\t\tvar tags = IgnoreTags.Split( ' ', StringSplitOptions.RemoveEmptyEntries );\r\n\t\tif ( tags.Length > 0 )\r\n\t\t\ttrace = trace.WithoutTags( tags );\r\n\r\n\t\tvar tr = trace.Run();\r\n\t\treturn (tr.Hit, tr.HitPosition, tr.Normal);\r\n\t}\r\n\r\n\t[MemberNotNullWhen( true, nameof( Renderer ) )]\r\n\tprivate bool EnsureResolved()\r\n\t{\r\n\t\tif ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( LeftFootBone ) || string.IsNullOrEmpty( RightFootBone ) )\r\n\t\t\treturn false;\r\n\r\n\t\tvar signature = (Renderer, LeftFootBone, RightFootBone, LeftRootOverride, LeftMidOverride, RightRootOverride, RightMidOverride, PelvisBoneOverride, LeftPlantBone, RightPlantBone);\r\n\t\tif ( signature.Equals( _cachedSignature ) && _leftRoot is not null )\r\n\t\t\treturn true;\r\n\r\n\t\tvar bones = Renderer.Model.Bones;\r\n\r\n\t\tif ( !TryResolveLeg( bones, LeftFootBone, LeftRootOverride, LeftMidOverride, out var leftRootNode, out _leftRoot, out _leftMid, out _leftEnd, out _leftBindPose ) )\r\n\t\t\treturn false;\r\n\r\n\t\tif ( !TryResolveLeg( bones, RightFootBone, RightRootOverride, RightMidOverride, out var rightRootNode, out _rightRoot, out _rightMid, out _rightEnd, out _rightBindPose ) )\r\n\t\t\treturn false;\r\n\r\n\t\tResolvePelvis( bones, leftRootNode, rightRootNode );\r\n\t\tResolvePlantBones( bones );\r\n\r\n\t\t_cachedSignature = signature;\r\n\t\t_smoothPelvis = 0f;\r\n\t\t_smoothDeltaL = 0f;\r\n\t\t_smoothDeltaR = 0f;\r\n\t\t_smoothPlantL = 0f;\r\n\t\t_smoothPlantR = 0f;\r\n\t\t_plantWindowL?.Reset();\r\n\t\t_plantWindowR?.Reset();\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\t// Plant points (ball / toe bone) whose ground contact is planted. Optional: an unset or\r\n\t// absent name falls back to the leg's own end bone at solve time (_*PlantResolved = false).\r\n\tprivate void ResolvePlantBones( BoneCollection bones )\r\n\t{\r\n\t\t_leftPlantResolved = false;\r\n\t\t_rightPlantResolved = false;\r\n\r\n\t\tif ( !string.IsNullOrEmpty( LeftPlantBone ) && bones.HasBone( LeftPlantBone ) )\r\n\t\t{\r\n\t\t\t_leftPlant = bones.GetBone( LeftPlantBone );\r\n\t\t\t_leftPlantResolved = true;\r\n\t\t}\r\n\r\n\t\tif ( !string.IsNullOrEmpty( RightPlantBone ) && bones.HasBone( RightPlantBone ) )\r\n\t\t{\r\n\t\t\t_rightPlant = bones.GetBone( RightPlantBone );\r\n\t\t\t_rightPlantResolved = true;\r\n\t\t}\r\n\t}\r\n\r\n\tprivate static bool TryResolveLeg( BoneCollection bones, string endBoneName, string rootOverrideName, string midOverrideName,\r\n\t\tout IBoneNode? rootNode, out BoneCollection.Bone rootBone, out BoneCollection.Bone midBone, out BoneCollection.Bone endBone,\r\n\t\tout BindPoseData bindPose )\r\n\t{\r\n\t\trootNode = null;\r\n\t\trootBone = null!;\r\n\t\tmidBone = null!;\r\n\t\tendBone = null!;\r\n\t\tbindPose = default;\r\n\r\n\t\tif ( !bones.HasBone( endBoneName ) )\r\n\t\t\treturn false;\r\n\r\n\t\tIBoneNode endNode = new SandboxBoneNode( bones.GetBone( endBoneName ) );\r\n\r\n\t\tIBoneNode? rootOverrideNode = null;\r\n\t\tif ( !string.IsNullOrEmpty( rootOverrideName ) )\r\n\t\t{\r\n\t\t\tif ( !bones.HasBone( rootOverrideName ) )\r\n\t\t\t\treturn false;\r\n\t\t\trootOverrideNode = new SandboxBoneNode( bones.GetBone( rootOverrideName ) );\r\n\t\t}\r\n\r\n\t\tIBoneNode? midOverrideNode = null;\r\n\t\tif ( !string.IsNullOrEmpty( midOverrideName ) )\r\n\t\t{\r\n\t\t\tif ( !bones.HasBone( midOverrideName ) )\r\n\t\t\t\treturn false;\r\n\t\t\tmidOverrideNode = new SandboxBoneNode( bones.GetBone( midOverrideName ) );\r\n\t\t}\r\n\r\n\t\tvar chain = BoneChainResolver.Resolve( endNode, rootOverrideNode, midOverrideNode );\r\n\t\tif ( !chain.Success )\r\n\t\t\treturn false;\r\n\r\n\t\trootNode = chain.Root;\r\n\t\trootBone = ((SandboxBoneNode)chain.Root!).Bone;\r\n\t\tmidBone = ((SandboxBoneNode)chain.Mid!).Bone;\r\n\t\tendBone = ((SandboxBoneNode)chain.End!).Bone;\r\n\r\n\t\tvar rootBindWorld = global::Transform.Zero;\r\n\t\tvar midBindWorld = global::Transform.Concat( rootBindWorld, midBone.LocalTransform );\r\n\t\tvar endBindWorld = global::Transform.Concat( midBindWorld, endBone.LocalTransform );\r\n\r\n\t\tbindPose = TwoBoneIkSolver.AnalyzeBindPose(\r\n\t\t\trootBindWorld.Position.ToNumerics(),\r\n\t\t\tmidBindWorld.Position.ToNumerics(),\r\n\t\t\tendBindWorld.Position.ToNumerics() );\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\tprivate void ResolvePelvis( BoneCollection bones, IBoneNode? leftRootNode, IBoneNode? rightRootNode )\r\n\t{\r\n\t\tif ( !string.IsNullOrEmpty( PelvisBoneOverride ) )\r\n\t\t{\r\n\t\t\tif ( bones.HasBone( PelvisBoneOverride ) )\r\n\t\t\t{\r\n\t\t\t\tvar pelvisBone = bones.GetBone( PelvisBoneOverride );\r\n\t\t\t\tif ( !IsChainBone( pelvisBone.Name ) )\r\n\t\t\t\t{\r\n\t\t\t\t\t_pelvisBone = pelvisBone;\r\n\t\t\t\t\tPelvisResolved = true;\r\n\t\t\t\t\treturn;\r\n\t\t\t\t}\r\n\t\t\t}\r\n\r\n\t\t\tPelvisResolved = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tif ( leftRootNode is not null && rightRootNode is not null )\r\n\t\t{\r\n\t\t\tvar ancestor = BoneChainResolver.FindCommonAncestor( leftRootNode, rightRootNode );\r\n\t\t\tif ( ancestor is not null && !IsChainBone( ancestor.Name ) && bones.HasBone( ancestor.Name ) )\r\n\t\t\t{\r\n\t\t\t\t_pelvisBone = bones.GetBone( ancestor.Name );\r\n\t\t\t\tPelvisResolved = true;\r\n\t\t\t\treturn;\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\tPelvisResolved = false;\r\n\t}\r\n\r\n\tprivate bool IsChainBone( string name )\r\n\t\t=> name == _leftRoot.Name || name == _leftMid.Name || name == _leftEnd.Name\r\n\t\t|| name == _rightRoot.Name || name == _rightMid.Name || name == _rightEnd.Name;\r\n\r\n\tprotected override void DrawGizmos()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tGizmo.Draw.Color = Color.Red;\r\n\t\t\tGizmo.Draw.WorldText( \"FootPlacementIK: invalid leg chains\", new global::Transform( WorldPosition ) );\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _leftEnd, out var leftEndTx );\r\n\t\tRenderer.TryGetBoneTransformAnimation( in _rightEnd, out var rightEndTx );\r\n\r\n\t\tVector3 up = Vector3.Up;\r\n\t\tVector3 origin = Renderer.WorldPosition;\r\n\r\n\t\tDrawFootGizmo( leftEndTx.Position, up, origin, _smoothDeltaL );\r\n\t\tDrawFootGizmo( rightEndTx.Position, up, origin, _smoothDeltaR );\r\n\r\n\t\tif ( PelvisResolved )\r\n\t\t{\r\n\t\t\tRenderer.TryGetBoneTransformAnimation( in _pelvisBone, out var pelvisTx );\r\n\t\t\tif ( MathF.Abs( _smoothPelvis ) > 0.01f )\r\n\t\t\t{\r\n\t\t\t\tGizmo.Draw.Color = Color.Magenta;\r\n\t\t\t\tGizmo.Draw.Arrow( pelvisTx.Position, pelvisTx.Position + up * _smoothPelvis, 2f, 1f );\r\n\t\t\t}\r\n\t\t}\r\n\t\telse\r\n\t\t{\r\n\t\t\tGizmo.Draw.Color = Color.Orange;\r\n\t\t\tGizmo.Draw.WorldText( \"FootPlacementIK: no pelvis resolved, feet-only mode (see PelvisBoneOverride)\", new global::Transform( WorldPosition + Vector3.Up * 8f ) );\r\n\t\t}\r\n\t}\r\n\r\n\tprivate void DrawFootGizmo( Vector3 footPos, Vector3 up, Vector3 origin, float smoothDelta )\r\n\t{\r\n\t\tvar trace = TraceFoot( footPos, up, origin );\r\n\r\n\t\tconst float traceMargin = 2f;\r\n\t\tVector3 footOnPlane = footPos - up * Vector3.Dot( footPos - origin, up );\r\n\t\tVector3 traceStart = footOnPlane + up * (MaxStepUp + traceMargin);\r\n\t\tVector3 traceEnd = footOnPlane - up * (MaxStepDown + traceMargin);\r\n\r\n\t\tGizmo.Draw.Color = trace.Hit ? Color.Green : Color.Red;\r\n\t\tGizmo.Draw.Line( traceStart, traceEnd );\r\n\r\n\t\tif ( trace.Hit )\r\n\t\t{\r\n\t\t\tGizmo.Draw.LineSphere( new Sphere( trace.HitPosition, 1.5f ) );\r\n\t\t\tGizmo.Draw.Arrow( trace.HitPosition, trace.HitPosition + trace.Normal * 6f, 1.5f, 1f );\r\n\r\n\t\t\tGizmo.Draw.Color = Color.Cyan;\r\n\t\t\tGizmo.Draw.LineSphere( new Sphere( footPos + up * smoothDelta, 1.5f ) );\r\n\t\t}\r\n\t}\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/Maths/FootPlacementInput.cs",
"FileName": "FootPlacementInput.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\nusing Vector3 = System.Numerics.Vector3;\r\n\r\n/// <summary>Per-frame input to <see cref=\"FootPlacementSolver.Solve\"/>. Angles are radians.</summary>\r\npublic struct FootPlacementInput\r\n{\r\n public FootInput LeftFoot;\r\n public FootInput RightFoot;\r\n\r\n /// <summary>Character ground-plane reference (typically the model root world position).</summary>\r\n public Vector3 OriginPosition;\r\n\r\n /// <summary>World up. Normalized by the solver.</summary>\r\n public Vector3 UpAxis;\r\n\r\n /// <summary>Additive raise applied to every grounded foot's delta.</summary>\r\n public float FootHeightOffset;\r\n\r\n /// <summary>Max positive (upward) vertical correction per foot.</summary>\r\n public float MaxStepUp;\r\n\r\n /// <summary>Max negative (downward) vertical correction per foot, as a positive number.</summary>\r\n public float MaxStepDown;\r\n\r\n /// <summary>Max pelvis drop, as a positive number.</summary>\r\n public float MaxPelvisDrop;\r\n\r\n /// <summary>Max pelvis raise, usually 0.</summary>\r\n public float MaxPelvisRaise;\r\n\r\n /// <summary>Clamp on how far a foot's rotation may tilt to align with the ground normal.</summary>\r\n public float MaxFootRotationRadians;\r\n\r\n /// <summary>Ground hits steeper than this (measured from up) are treated as no-hit.</summary>\r\n public float MaxGroundSlopeRadians;\r\n\r\n /// <summary>0-1. Applied ONLY to <see cref=\"FootPlacementResult.PelvisOffset\"/> - per-foot\r\n /// results are unweighted, so the caller's own two-bone solve (with its own tested,\r\n /// endpoint-exact weight blend) is the single place feet actually get blended.</summary>\r\n public float Weight;\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/Maths/FootPlacementResult.cs",
"FileName": "FootPlacementResult.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\n/// <summary>Output of <see cref=\"FootPlacementSolver.Solve\"/>.</summary>\r\npublic struct FootPlacementResult\r\n{\r\n public FootResult LeftFoot;\r\n public FootResult RightFoot;\r\n\r\n /// <summary>Clamped AND weight-scaled, measured along the up axis. Final, write-ready -\r\n /// unlike the per-foot results, this already has <see cref=\"FootPlacementInput.Weight\"/>\r\n /// applied (see that type's doc comment for why the split is asymmetric).</summary>\r\n public float PelvisOffset;\r\n\r\n /// <summary>False only when <see cref=\"FootPlacementInput.UpAxis\"/> is degenerate.</summary>\r\n public bool Solved;\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/Assembly.cs",
"FileName": "Assembly.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable annotations\r\n\r\n// Global usings for the s&box in-engine compiler.\r\n//\r\n// The plain net8.0 dev harness gets these automatically via <ImplicitUsings>,\r\n// but s&box's compiler injects no BCL usings at all - without this file the\r\n// library fails to compile inside the editor.\r\n\r\nglobal using System;\r\nglobal using System.Collections.Generic;\r\nglobal using System.Linq;\r\n\r\n// NOTE on Vector3/Quaternion: s&box declares its own Vector3 and Rotation in the\r\n// global namespace, which wins over `using System.Numerics;` during name lookup.\r\n// The engine-agnostic math in Maths/ uses System.Numerics types exclusively so it\r\n// also compiles in the plain net8.0 dev harness (dev/BetterIk.Dev.csproj). Every\r\n// file under Maths/ that uses the simple name Vector3 or Quaternion carries a\r\n// namespace-scoped alias after its file-scoped namespace line, e.g.:\r\n//\r\n// namespace BetterIk.Maths;\r\n// using Vector3 = System.Numerics.Vector3;\r\n//\r\n// A global alias does not work here (CS0576: conflicts with the global-namespace\r\n// type at every use site), the alias must be namespace-scoped per file.\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/Maths/FabrikSolver.cs",
"FileName": "FabrikSolver.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\nusing Vector3 = System.Numerics.Vector3;\r\nusing Quaternion = System.Numerics.Quaternion;\r\n\r\n/// <summary>\r\n/// Engine-agnostic, unconstrained FABRIK solver for variable-length chains (tails, tentacles,\r\n/// ropes). No pole vectors, no joint constraints/limits, no per-joint stiffness in this version -\r\n/// use TwoBoneIK for hinge-like limb behavior; this is for chains that don't need it. Pure and\r\n/// stateless: identical input always produces identical output, safe to call every frame.\r\n/// </summary>\r\npublic static class FabrikSolver\r\n{\r\n private const float TinyLenSq = 1e-12f;\r\n\r\n public static FabrikResult Solve(in FabrikInput input)\r\n {\r\n Vector3[] animated = input.JointPositions;\r\n int n = animated.Length;\r\n\r\n float weight = Math.Clamp(input.Weight, 0f, 1f);\r\n\r\n // Structural early-out: exact passthrough, no solve runs at all.\r\n if (weight <= 0f)\r\n {\r\n return new FabrikResult\r\n {\r\n JointPositions = CopyArray(animated),\r\n IterationsUsed = 0,\r\n Converged = false,\r\n };\r\n }\r\n\r\n float[] segLengths = new float[n - 1];\r\n float totalLength = 0f;\r\n for (int i = 0; i < n - 1; i++)\r\n {\r\n segLengths[i] = (animated[i + 1] - animated[i]).Length();\r\n totalLength += segLengths[i];\r\n }\r\n\r\n Vector3 root = animated[0];\r\n Vector3 toTarget = input.TargetPosition - root;\r\n float rootToTargetDist = toTarget.Length();\r\n\r\n Vector3[] solved;\r\n int iterationsUsed;\r\n bool converged;\r\n\r\n // Target coincides with root: aim direction is undefined, passthrough unchanged rather\r\n // than dividing by a zero-length vector.\r\n if (rootToTargetDist < 1e-5f)\r\n {\r\n solved = CopyArray(animated);\r\n iterationsUsed = 0;\r\n converged = false;\r\n }\r\n else if (rootToTargetDist >= totalLength)\r\n {\r\n // Unreachable: analytic straight chain along the root-to-target ray, deterministic.\r\n Vector3 dir = toTarget / rootToTargetDist;\r\n solved = new Vector3[n];\r\n solved[0] = root;\r\n float cumulative = 0f;\r\n for (int i = 1; i < n; i++)\r\n {\r\n cumulative += segLengths[i - 1];\r\n solved[i] = root + dir * cumulative;\r\n }\r\n iterationsUsed = 0;\r\n converged = false;\r\n }\r\n else\r\n {\r\n solved = CopyArray(animated);\r\n float tolerance = MathF.Max(input.Tolerance, 1e-6f);\r\n int maxIterations = Math.Max(input.MaxIterations, 1);\r\n\r\n converged = false;\r\n iterationsUsed = 0;\r\n\r\n for (int iter = 0; iter < maxIterations; iter++)\r\n {\r\n iterationsUsed = iter + 1;\r\n\r\n // Backward pass: pin the end to the target, walk toward the root re-fixing lengths.\r\n solved[n - 1] = input.TargetPosition;\r\n for (int i = n - 2; i >= 0; i--)\r\n {\r\n Vector3 dir = SafeDirection(solved[i] - solved[i + 1]);\r\n solved[i] = solved[i + 1] + dir * segLengths[i];\r\n }\r\n\r\n // Forward pass: re-pin the root, walk toward the end re-fixing lengths.\r\n solved[0] = root;\r\n for (int i = 1; i < n; i++)\r\n {\r\n Vector3 dir = SafeDirection(solved[i] - solved[i - 1]);\r\n solved[i] = solved[i - 1] + dir * segLengths[i - 1];\r\n }\r\n\r\n if ((solved[n - 1] - input.TargetPosition).Length() <= tolerance)\r\n {\r\n converged = true;\r\n break;\r\n }\r\n }\r\n }\r\n\r\n if (weight >= 1f)\r\n {\r\n return new FabrikResult { JointPositions = solved, IterationsUsed = iterationsUsed, Converged = converged };\r\n }\r\n\r\n // Position-lerp blend. This does not preserve segment lengths at intermediate weights -\r\n // accepted tradeoff, same class as TwoBoneIK's weight blend; tails are visually forgiving.\r\n var blended = new Vector3[n];\r\n for (int i = 0; i < n; i++)\r\n blended[i] = Vector3.Lerp(animated[i], solved[i], weight);\r\n\r\n return new FabrikResult { JointPositions = blended, IterationsUsed = iterationsUsed, Converged = converged };\r\n }\r\n\r\n /// <summary>\r\n /// Derives per-joint world rotations from the change in segment direction between the animated\r\n /// and solved poses, via the same shortest-arc delta-rotation philosophy already proven under\r\n /// bone roll (IkMath.FromToRotation). No twist/roll control in this version - long chains under\r\n /// large deflection can accumulate visually odd roll; acceptable for v1, a twist-distribution\r\n /// pass is a possible later addition. The leaf bone (last index) has no segment of its own, so\r\n /// it is a documented convention, not a derived truth: it reuses the last real segment's delta.\r\n /// </summary>\r\n public static Quaternion[] DeriveRotations(Vector3[] animatedPositions, Vector3[] solvedPositions, Quaternion[] animatedRotations)\r\n {\r\n int n = animatedPositions.Length;\r\n var result = new Quaternion[n];\r\n Quaternion lastDelta = Quaternion.Identity;\r\n\r\n for (int i = 0; i < n - 1; i++)\r\n {\r\n Vector3 animatedDir = SafeDirection(animatedPositions[i + 1] - animatedPositions[i]);\r\n Vector3 solvedDir = SafeDirection(solvedPositions[i + 1] - solvedPositions[i]);\r\n lastDelta = IkMath.FromToRotation(animatedDir, solvedDir);\r\n result[i] = Quaternion.Normalize(lastDelta * animatedRotations[i]);\r\n }\r\n\r\n // Leaf bone convention: reuse the last real segment's delta, applied to the leaf's own\r\n // animated rotation (not the previous joint's).\r\n result[n - 1] = Quaternion.Normalize(lastDelta * animatedRotations[n - 1]);\r\n return result;\r\n }\r\n\r\n private static Vector3 SafeDirection(Vector3 v)\r\n {\r\n float lenSq = v.LengthSquared();\r\n return lenSq < TinyLenSq ? Vector3.UnitX : v / MathF.Sqrt(lenSq);\r\n }\r\n\r\n // Array.Clone() is not on s&box's code whitelist for the game-code assembly (confirmed via\r\n // a live compile attempt: \"System.Array.Clone() is not allowed when whitelist is enabled\").\r\n // A manual element-by-element copy is the whitelist-safe equivalent.\r\n private static Vector3[] CopyArray(Vector3[] source)\r\n {\r\n var copy = new Vector3[source.Length];\r\n for (int i = 0; i < source.Length; i++)\r\n copy[i] = source[i];\r\n return copy;\r\n }\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/Maths/TwoBoneIkSolver.cs",
"FileName": "TwoBoneIkSolver.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\nusing Vector3 = System.Numerics.Vector3;\r\nusing Quaternion = System.Numerics.Quaternion;\r\n\r\n/// <summary>\r\n/// Engine-agnostic, closed-form two-bone IK solver with pole vector control. Pure and stateless:\r\n/// identical input always produces identical output, safe to call every frame for runtime blending.\r\n/// </summary>\r\npublic static class TwoBoneIkSolver\r\n{\r\n // All thresholds below are relative to chain length (Lmax) or another geometric quantity,\r\n // never absolute, so the solver is scale-equivariant.\r\n private const float TinyRel = 1e-6f;\r\n\r\n public static BindPoseData AnalyzeBindPose(Vector3 rootPos, Vector3 midPos, Vector3 endPos)\r\n {\r\n float l1 = (midPos - rootPos).Length();\r\n float l2 = (endPos - midPos).Length();\r\n float lengthSum = MathF.Max(l1 + l2, 1e-8f);\r\n\r\n Vector3 chainDir = IkMath.SafeNormalize(endPos - rootPos, Vector3.UnitX);\r\n Vector3 elbowOffset = IkMath.ProjectPerpendicular(midPos - rootPos, chainDir);\r\n\r\n float threshold = 1e-4f * lengthSum;\r\n\r\n if (elbowOffset.Length() >= threshold)\r\n {\r\n Vector3 poleDir = Vector3.Normalize(elbowOffset);\r\n Vector3 bendNormal = IkMath.SafeNormalize(\r\n Vector3.Cross(endPos - rootPos, midPos - rootPos),\r\n IkMath.AnyPerpendicular(chainDir));\r\n return new BindPoseData(l1, l2, bendNormal, poleDir, true);\r\n }\r\n else\r\n {\r\n Vector3 poleDir = IkMath.AnyPerpendicular(chainDir);\r\n Vector3 bendNormal = Vector3.Normalize(Vector3.Cross(chainDir, poleDir));\r\n return new BindPoseData(l1, l2, bendNormal, poleDir, false);\r\n }\r\n }\r\n\r\n public static TwoBoneIkResult Solve(in TwoBoneIkInput input)\r\n {\r\n Vector3 a = input.RootPosition;\r\n Vector3 b = input.MidPosition;\r\n Vector3 c = input.EndPosition;\r\n\r\n float l1 = (b - a).Length();\r\n float l2 = (c - b).Length();\r\n float lmax = l1 + l2;\r\n\r\n float wMaster = Math.Clamp(input.MasterWeight, 0f, 1f);\r\n float wPos = wMaster * Math.Clamp(input.PositionWeight, 0f, 1f);\r\n float wRot = wMaster * Math.Clamp(input.RotationWeight, 0f, 1f);\r\n\r\n Quaternion endRotation = Quaternion.Normalize(BlendRotation(input.EndRotation, input.TargetRotation, wRot));\r\n\r\n // Degenerate chain: at least one bone effectively zero length. Rotation goal is independent\r\n // of chain geometry, so it still applies; position/root/mid pass through unmodified.\r\n if (lmax < 1e-8f || l1 < TinyRel * lmax || l2 < TinyRel * lmax)\r\n {\r\n return new TwoBoneIkResult\r\n {\r\n RootRotation = input.RootRotation,\r\n MidRotation = input.MidRotation,\r\n EndRotation = endRotation,\r\n MidPosition = b,\r\n EndPosition = c,\r\n AppliedStretch = 1f,\r\n Solved = false,\r\n };\r\n }\r\n\r\n Vector3 toTarget = input.TargetPosition - a;\r\n float d = toTarget.Length();\r\n\r\n // Target coincides with root: root-to-target direction is undefined, fall back to the\r\n // animated chain direction (never NaN; continuity through this exact point is not required).\r\n Vector3 aHat = d < TinyRel * lmax\r\n ? IkMath.SafeNormalize(c - a, Vector3.UnitX)\r\n : toTarget / d;\r\n\r\n (float dEff, float sFull, float dFinal) = SolveReach(d, lmax, input.SoftFraction, input.MaxStretch);\r\n\r\n float l1s = sFull * l1;\r\n float l2s = sFull * l2;\r\n\r\n // Near-side clamp: when bone lengths differ, the chain also cannot reach closer than\r\n // |l1s - l2s| (folding the longer bone back over the shorter one). Mirrors the far-side\r\n // max-reach clamp; without it cosAlpha blows outside [-1,1] and the elbow snaps to an\r\n // inconsistent pose whose end position drifts off target. A near-side reach clamp found\r\n // by fuzz testing, not covered by the original edge-case table (which only listed\r\n // \"beyond max reach\" and \"exactly at zero\").\r\n float minReach = MathF.Abs(l1s - l2s);\r\n if (dFinal < minReach)\r\n dFinal = minReach;\r\n\r\n Vector3 dHat = SelectElbowDirection(input, a, b, aHat, lmax);\r\n\r\n float alpha = SolveRootAngle(dFinal, l1s, l2s, lmax);\r\n\r\n Vector3 midSolved = a + l1s * (MathF.Cos(alpha) * aHat + MathF.Sin(alpha) * dHat);\r\n Vector3 endSolved = a + dFinal * aHat;\r\n\r\n Vector3 rawPoseNormal = Vector3.Cross(c - a, b - a);\r\n Vector3 oldNormalHint = rawPoseNormal.LengthSquared() >= (1e-5f * l1 * l2) * (1e-5f * l1 * l2)\r\n ? rawPoseNormal\r\n : input.FallbackBendNormal;\r\n\r\n Vector3 newNormalHint = Vector3.Cross(aHat, dHat);\r\n\r\n Quaternion deltaRootFull = IkMath.DeltaRotation(b - a, oldNormalHint, midSolved - a, newNormalHint);\r\n Quaternion deltaMidFull = IkMath.DeltaRotation(c - b, oldNormalHint, endSolved - midSolved, newNormalHint);\r\n\r\n Quaternion deltaRoot = BlendRotation(Quaternion.Identity, deltaRootFull, wPos);\r\n Quaternion deltaMid = BlendRotation(Quaternion.Identity, deltaMidFull, wPos);\r\n\r\n float sBlended = 1f + (sFull - 1f) * wPos;\r\n\r\n Vector3 midBlended = a + sBlended * Vector3.Transform(b - a, deltaRoot);\r\n Vector3 endBlended = midBlended + sBlended * Vector3.Transform(c - b, deltaMid);\r\n\r\n return new TwoBoneIkResult\r\n {\r\n RootRotation = Quaternion.Normalize(deltaRoot * input.RootRotation),\r\n MidRotation = Quaternion.Normalize(deltaMid * input.MidRotation),\r\n EndRotation = endRotation,\r\n MidPosition = midBlended,\r\n EndPosition = endBlended,\r\n AppliedStretch = sBlended,\r\n Solved = true,\r\n };\r\n }\r\n\r\n // Exact at t=0/t=1 (no Slerp numerical noise at the endpoints); Slerp in between.\r\n private static Quaternion BlendRotation(Quaternion from, Quaternion to, float t)\r\n {\r\n if (t <= 0f) return from;\r\n if (t >= 1f) return to;\r\n return Quaternion.Slerp(from, to, t);\r\n }\r\n\r\n // Soft clamp (exponential falloff) + stretch. Returns the reach distance actually solved for\r\n // (dEff), the blended-in stretch scale (sFull), and the final root-to-end distance (dFinal).\r\n private static (float dEff, float sFull, float dFinal) SolveReach(float d, float lmax, float softFractionRaw, float maxStretchRaw)\r\n {\r\n float softFraction = Math.Clamp(softFractionRaw, 0f, 0.499f);\r\n float maxStretch = MathF.Max(maxStretchRaw, 0f);\r\n\r\n float dEff;\r\n if (softFraction < 1e-4f)\r\n {\r\n dEff = MathF.Min(d, lmax);\r\n }\r\n else\r\n {\r\n float dSoft = (1f - softFraction) * lmax;\r\n float r = softFraction * lmax;\r\n dEff = d <= dSoft ? d : dSoft + r * (1f - MathF.Exp(-(d - dSoft) / r));\r\n }\r\n\r\n float sFull = dEff > 0f ? Math.Clamp(d / dEff, 1f, 1f + maxStretch) : 1f;\r\n float dFinal = MathF.Min(d, dEff * sFull);\r\n\r\n return (dEff, sFull, dFinal);\r\n }\r\n\r\n // Law of cosines for the angle at the root, guarded against the near-zero-reach fold singularity\r\n // (where the denominator would be zero); alpha = pi/2 there safely folds the chain via dHat.\r\n private static float SolveRootAngle(float dFinal, float l1s, float l2s, float lmax)\r\n {\r\n if (dFinal < TinyRel * lmax || l1s < TinyRel * lmax)\r\n return MathF.PI / 2f;\r\n\r\n float cosAlpha = (dFinal * dFinal + l1s * l1s - l2s * l2s) / (2f * dFinal * l1s);\r\n return MathF.Acos(Math.Clamp(cosAlpha, -1f, 1f));\r\n }\r\n\r\n // Pole hint -> pose-derived elbow offset -> fallback bend normal -> arbitrary perpendicular.\r\n private static Vector3 SelectElbowDirection(in TwoBoneIkInput input, Vector3 a, Vector3 b, Vector3 aHat, float lmax)\r\n {\r\n if (input.HasPole)\r\n {\r\n Vector3 pPerp = IkMath.ProjectPerpendicular(input.PoleHint, aHat);\r\n float threshold = 1e-4f * MathF.Max(input.PoleHint.Length(), lmax);\r\n if (pPerp.Length() >= threshold)\r\n return ApplyOffset(Vector3.Normalize(pPerp), aHat, input.PoleAngleOffsetRadians);\r\n }\r\n\r\n Vector3 mPerp = IkMath.ProjectPerpendicular(b - a, aHat);\r\n if (mPerp.Length() >= 1e-5f * lmax)\r\n return ApplyOffset(Vector3.Normalize(mPerp), aHat, input.PoleAngleOffsetRadians);\r\n\r\n // FallbackBendNormal is a plane normal, not an in-plane direction: cross with aHat to get\r\n // the in-plane, perpendicular-to-aHat elbow direction it implies.\r\n Vector3 crossFallback = Vector3.Cross(input.FallbackBendNormal, aHat);\r\n Vector3 dHat = crossFallback.LengthSquared() >= 1e-10f\r\n ? Vector3.Normalize(crossFallback)\r\n : IkMath.AnyPerpendicular(aHat);\r\n\r\n return ApplyOffset(dHat, aHat, input.PoleAngleOffsetRadians);\r\n }\r\n\r\n private static Vector3 ApplyOffset(Vector3 dHat, Vector3 axis, float angleRadians)\r\n {\r\n if (angleRadians == 0f)\r\n return dHat;\r\n return Vector3.Transform(dHat, Quaternion.CreateFromAxisAngle(axis, angleRadians));\r\n }\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "BetterIk/SandboxBoneNode.cs",
"FileName": "SandboxBoneNode.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nusing Sandbox;\r\nusing BetterIk.Skeleton;\r\n\r\nnamespace BetterIk;\r\n\r\n/// <summary>Thin adapter over the real engine BoneCollection.Bone, exposing just enough to\r\n/// let BoneChainResolver walk the skeleton. No logic beyond delegation - not unit tested.</summary>\r\ninternal sealed class SandboxBoneNode : IBoneNode\r\n{\r\n public SandboxBoneNode(BoneCollection.Bone bone)\r\n {\r\n Bone = bone;\r\n }\r\n\r\n public BoneCollection.Bone Bone { get; }\r\n\r\n public string Name => Bone.Name;\r\n\r\n public IBoneNode? Parent => Bone.Parent is null ? null : new SandboxBoneNode(Bone.Parent);\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/FabrikIK.cs",
"FileName": "FabrikIK.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable\r\n\r\nusing System.Diagnostics.CodeAnalysis;\r\nusing System.Linq;\r\nusing Sandbox;\r\nusing BetterIk.Maths;\r\nusing BetterIk.Skeleton;\r\n\r\nnamespace BetterIk;\r\n\r\n/// <summary>\r\n/// Unconstrained FABRIK for variable-length chains (tails, tentacles, ropes). No pole vector, no\r\n/// joint limits, no per-joint stiffness - for hinge-like limb IK, use TwoBoneIK instead. Drop on\r\n/// the model root (or any descendant), set RootBone and EndBone to the chain's two ends; unlike\r\n/// TwoBoneIK this cannot auto-walk a fixed depth, since a FABRIK chain's length is arbitrary, so\r\n/// RootBone must be named explicitly.\r\n/// </summary>\r\npublic sealed class FabrikIK : Component, IHasSkinnedRenderer\r\n{\r\n\t[Property] public SkinnedModelRenderer? Renderer { get; set; }\r\n\t[Property, BoneName] public string RootBone { get; set; } = \"\";\r\n\t[Property, BoneName] public string EndBone { get; set; } = \"\";\r\n\t[Property] public GameObject? Target { get; set; }\r\n\r\n\t[Property, Range( 0f, 1f )] public float Weight { get; set; } = 1f;\r\n\r\n\t[Property, Group( \"Advanced\" )] public int MaxIterations { get; set; } = 16;\r\n\r\n\t// 0 = auto: 0.001 * total chain length, computed at solve time from the animated pose.\r\n\t[Property, Group( \"Advanced\" )] public float Tolerance { get; set; } = 0f;\r\n\r\n\tpublic bool HasValidChain { get; private set; }\r\n\tpublic bool LastSolveApplied { get; private set; }\r\n\r\n\tprivate (SkinnedModelRenderer Renderer, string RootBone, string EndBone) _cachedSignature;\r\n\t// Root-first, end-last. Only valid once EnsureResolved() has returned true at least once.\r\n\tprivate BoneCollection.Bone[] _chainBones = null!;\r\n\tprivate Vector3[] _chainScales = null!;\r\n\r\n\tprotected override void OnStart()\r\n\t{\r\n\t\tRenderer ??= GameObject.GetComponent<SkinnedModelRenderer>()\r\n\t\t\t?? GameObject.GetComponentInParent<SkinnedModelRenderer>( includeSelf: true );\r\n\t}\r\n\r\n\tprotected override void OnPreRender()\r\n\t{\r\n\t\tSolve();\r\n\t}\r\n\r\n\tprivate void Solve()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tHasValidChain = false;\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tHasValidChain = true;\r\n\r\n\t\tif ( Target is null || !Target.IsValid )\r\n\t\t{\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\t// Weight <= 0 skips the read-solve-write cycle entirely rather than reading the animated\r\n\t\t// pose and writing the identical value back. Live testing found the latter is NOT\r\n\t\t// perfectly lossless through the world<->model-local round trip (Renderer.ToModelLocal)\r\n\t\t// every frame: on Rig B's tail, repeatedly reading TryGetBoneTransformAnimation then\r\n\t\t// writing the visually-unchanged result back via SetBoneTransform compounded into\r\n\t\t// Infinity within a few seconds, even with bone scale frozen (Renderer.ToModelLocal\r\n\t\t// finding above). Not writing at all when there is nothing to blend is strictly safer.\r\n\t\tif ( Weight <= 0f )\r\n\t\t{\r\n\t\t\tLastSolveApplied = false;\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tint n = _chainBones.Length;\r\n\t\tvar positions = new System.Numerics.Vector3[n];\r\n\t\tvar rotations = new System.Numerics.Quaternion[n];\r\n\r\n\t\tfloat totalLength = 0f;\r\n\t\tfor ( int i = 0; i < n; i++ )\r\n\t\t{\r\n\t\t\tRenderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );\r\n\t\t\tpositions[i] = tx.Position.ToNumerics();\r\n\t\t\trotations[i] = tx.Rotation.ToNumerics();\r\n\t\t\tif ( i > 0 )\r\n\t\t\t\ttotalLength += (positions[i] - positions[i - 1]).Length();\r\n\t\t}\r\n\r\n\t\tfloat tolerance = Tolerance > 0f ? Tolerance : 0.001f * MathF.Max( totalLength, 1e-4f );\r\n\r\n\t\tvar input = new FabrikInput\r\n\t\t{\r\n\t\t\tJointPositions = positions,\r\n\t\t\tTargetPosition = Target.WorldPosition.ToNumerics(),\r\n\t\t\tWeight = Weight,\r\n\t\t\tMaxIterations = MaxIterations,\r\n\t\t\tTolerance = tolerance,\r\n\t\t};\r\n\r\n\t\tvar result = FabrikSolver.Solve( input );\r\n\t\tvar solvedRotations = FabrikSolver.DeriveRotations( positions, result.JointPositions, rotations );\r\n\r\n\t\tfor ( int i = 0; i < n; i++ )\r\n\t\t{\r\n\t\t\tRenderer.SetBoneTransform( in _chainBones[i], Renderer.ToModelLocal(\r\n\t\t\t\tnew global::Transform( result.JointPositions[i].ToSandbox(), solvedRotations[i].ToSandbox() ).WithScale( _chainScales[i] ) ) );\r\n\t\t}\r\n\r\n\t\t// PostAnimationUpdate is [Obsolete] with no replacement documented in the shipped XML\r\n\t\t// docs; kept defensively, matching the other components' already-verified usage.\r\n#pragma warning disable CS0612\r\n\t\tRenderer.PostAnimationUpdate();\r\n#pragma warning restore CS0612\r\n\r\n\t\tLastSolveApplied = true;\r\n\t}\r\n\r\n\t[MemberNotNullWhen( true, nameof( Renderer ) )]\r\n\tprivate bool EnsureResolved()\r\n\t{\r\n\t\tif ( Renderer is null || Renderer.Model is null || string.IsNullOrEmpty( RootBone ) || string.IsNullOrEmpty( EndBone ) )\r\n\t\t\treturn false;\r\n\r\n\t\tvar signature = (Renderer, RootBone, EndBone);\r\n\t\tif ( signature.Equals( _cachedSignature ) && _chainBones is not null )\r\n\t\t\treturn true;\r\n\r\n\t\tvar bones = Renderer.Model.Bones;\r\n\t\tif ( !bones.HasBone( EndBone ) )\r\n\t\t\treturn false;\r\n\r\n\t\tIBoneNode endNode = new SandboxBoneNode( bones.GetBone( EndBone ) );\r\n\t\tvar chain = BoneChainResolver.ResolveNamedChain( endNode, RootBone );\r\n\t\tif ( !chain.Success )\r\n\t\t\treturn false;\r\n\r\n\t\t_chainBones = chain.Chain!.Select( node => ((SandboxBoneNode)node).Bone ).ToArray();\r\n\t\t_cachedSignature = signature;\r\n\r\n\t\t// Cached once here rather than re-read from TryGetBoneTransformAnimation every frame.\r\n\t\t// Re-reading and re-writing world scale every frame round-trips it through\r\n\t\t// Renderer.ToModelLocal on every solve; live testing found this compounds into Infinity\r\n\t\t// within a few seconds on Rig B's tail (bone scale is not exactly 1 on that rig). IK never\r\n\t\t// needs to change a bone's scale, so freezing it at first resolution is both safe and\r\n\t\t// removes the feedback path entirely.\r\n\t\t_chainScales = new Vector3[_chainBones.Length];\r\n\t\tfor ( int i = 0; i < _chainBones.Length; i++ )\r\n\t\t{\r\n\t\t\tRenderer.TryGetBoneTransformAnimation( in _chainBones[i], out var tx );\r\n\t\t\t_chainScales[i] = tx.Scale;\r\n\t\t}\r\n\r\n\t\treturn true;\r\n\t}\r\n\r\n\tprotected override void DrawGizmos()\r\n\t{\r\n\t\tif ( !EnsureResolved() )\r\n\t\t{\r\n\t\t\tGizmo.Draw.Color = Color.Red;\r\n\t\t\tGizmo.Draw.WorldText( \"FabrikIK: invalid chain\", new global::Transform( WorldPosition ) );\r\n\t\t\treturn;\r\n\t\t}\r\n\r\n\t\tif ( Target is null || !Target.IsValid )\r\n\t\t\treturn;\r\n\r\n\t\tGizmo.Draw.Color = Color.Yellow;\r\n\t\tfor ( int i = 0; i < _chainBones.Length - 1; i++ )\r\n\t\t{\r\n\t\t\tRenderer.TryGetBoneTransform( in _chainBones[i], out var a );\r\n\t\t\tRenderer.TryGetBoneTransform( in _chainBones[i + 1], out var b );\r\n\t\t\tGizmo.Draw.Line( a.Position, b.Position );\r\n\t\t}\r\n\r\n\t\tGizmo.Draw.Color = Color.White;\r\n\t\tGizmo.Draw.LineSphere( new Sphere( Target.WorldPosition, 2f ) );\r\n\t}\r\n}\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/Maths/AssemblyInfo.cs",
"FileName": "AssemblyInfo.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "#nullable enable annotations\r\n\r\n// Engine-agnostic IK math. No Sandbox/Editor references allowed in this folder:\r\n// these sources also compile in the plain net8.0 dev harness (dev/BetterIk.Dev.csproj)\r\n// so the solver can be unit-tested without the s&box editor running.\r\n"
},
{
"Ident": "notpointless.chomnr_better_ik",
"Path": "Code/BetterIk/Maths/FootResult.cs",
"FileName": "FootResult.cs",
"PackageType": "library",
"CodeKind": "Game",
"AssetVersionId": 311785,
"Code": "namespace BetterIk.Maths;\r\n\r\nusing Quaternion = System.Numerics.Quaternion;\r\n\r\n/// <summary>Per-foot output of <see cref=\"FootPlacementSolver.Solve\"/>. Unweighted - the\r\n/// aggregate <see cref=\"FootPlacementInput.Weight\"/> is applied only to\r\n/// <see cref=\"FootPlacementResult.PelvisOffset\"/>, not here (see that type's doc comment).</summary>\r\npublic readonly struct FootResult\r\n{\r\n /// <summary>True when a hit existed, its normal was usable, and the slope was within limit.</summary>\r\n public readonly bool Grounded;\r\n\r\n /// <summary>Clamped, unweighted, measured along the up axis. 0 when not grounded.</summary>\r\n public readonly float VerticalDelta;\r\n\r\n /// <summary>Ground-aligned foot rotation goal, unweighted. Equals the input FootRotation\r\n /// exactly when not grounded.</summary>\r\n public readonly Quaternion TargetRotation;\r\n\r\n public FootResult(bool grounded, float verticalDelta, Quaternion targetRotation)\r\n {\r\n Grounded = grounded;\r\n VerticalDelta = verticalDelta;\r\n TargetRotation = targetRotation;\r\n }\r\n}\r\n"
}
]
}