Merge branch 'ik' into main

Brio/Brio.cs

@@ -132,6 +132,7 @@ private IServiceCollection SetupServices(DalamudServices dalamudServices)
         serviceCollection.AddSingleton<WorldRenderingService>();
         serviceCollection.AddSingleton<SkeletonService>();
         serviceCollection.AddSingleton<PosingService>();
+        serviceCollection.AddSingleton<IKService>();
         serviceCollection.AddSingleton<CameraService>();
         serviceCollection.AddSingleton<ObjectMonitorService>();
 

Brio/Capabilities/Actor/ActorDebugCapability.cs

@@ -1,6 +1,8 @@
-using Brio.Config;
+using Brio.Capabilities.Posing;
+using Brio.Config;
 using Brio.Entities.Actor;
 using Brio.UI.Widgets.Actor;
+using System.Collections.Generic;
 
 namespace Brio.Capabilities.Actor;
 
@@ -17,4 +19,15 @@ public ActorDebugCapability(ActorEntity parent, ConfigurationService configServi
 
         Widget = new ActorDebugWidget(this);
     }
+
+    public Dictionary<string, int> SkeletonStacks
+    {
+        get
+        {
+            if(Entity.TryGetCapability<SkeletonPosingCapability>(out var capability))
+                return capability.PoseInfo.StackCounts;
+
+            return [];
+        }
+    }
 }

Brio/Capabilities/Posing/PosingCapability.cs

@@ -115,7 +115,7 @@ public void ExportPose(string path)
         ResourceProvider.Instance.SaveFileDocument(path, poseFile);
     }
 
-    public void Snapshot()
+    public async void Snapshot()
     {
         var undoStackSize = _configurationService.Configuration.Posing.UndoStackSize;
         if (undoStackSize <= 0)
@@ -125,6 +125,15 @@ public void Snapshot()
             return;
         }
 
+        if(SkeletonPosing.PoseInfo.HasIKStacks)
+        {
+            var all = new PoseImporterOptions(new BoneFilter(_posingService), TransformComponents.All, true);
+            var poseFile = await _framework.RunOnTick(() => GeneratePoseFile());
+            SkeletonPosing.PoseInfo.Clear();
+            ImportPose(poseFile, options: all, generateSnapshot: true);
+            return;
+        }
+
         if(!_undoStack.Any())
             _undoStack.Push(new PoseStack(new PoseInfo(), ModelPosing.OriginalTransform));
 

Brio/Core/NativeHelpers.cs

@@ -0,0 +1,27 @@
+
+using System.Runtime.InteropServices;
+using System;
+
+namespace Brio.Core;
+internal static class NativeHelpers
+{
+    public static (nint Aligned, nint Unaligned) AllocateAlignedMemory(int sizeInBytes, int alignment)
+    {
+        int alignedSize = sizeInBytes + alignment - 1;
+        nint unalignedMemory = Marshal.AllocHGlobal(alignedSize);
+        int alignmentOffset = (int)(alignment - (unalignedMemory % alignment));
+        nint alignedMemory = unalignedMemory + alignmentOffset;
+
+        return (alignedMemory, unalignedMemory);
+    }
+
+    public static void FreeAlignedMemory((nint Aligned, nint Unaligned) addrs)
+    {
+        Marshal.FreeHGlobal(addrs.Unaligned);
+    }
+
+    public static void FreeMemory(nint addr)
+    {
+        Marshal.FreeHGlobal(addr);
+    }
+}

Brio/Game/Posing/IKService.cs

@@ -0,0 +1,72 @@
+using Brio.Core;
+using Brio.Game.Posing.Skeletons;
+using Dalamud.Game;
+using FFXIVClientStructs.Havok;
+using System;
+using System.Numerics;
+using System.Runtime.InteropServices;
+
+namespace Brio.Game.Posing;
+internal unsafe class IKService : IDisposable
+{
+    delegate* unmanaged<hkaCCDSolver*, int, float, void> _ccdSolverCtr;
+    delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<IKConstraint>*, hkaPose*, byte*> _ccdSolverSolve;
+
+    private (nint Aligned, nint Unaligned) _solverAddr;
+    private (nint Aligned, nint Unaligned) _constraintAddr;
+
+    public IKService(ISigScanner scanner)
+    {
+        _ccdSolverCtr = (delegate* unmanaged<hkaCCDSolver*, int, float, void>)scanner.ScanText("E8 ?? ?? ?? ?? BA ?? ?? ?? ?? 48 C7 43");
+        _ccdSolverSolve = (delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<IKConstraint>*, hkaPose*, byte*>)scanner.ScanText("E8 ?? ?? ?? ?? 8B 45 ?? 48 8B 75");
+
+        _solverAddr = NativeHelpers.AllocateAlignedMemory(sizeof(hkaCCDSolver), 16);
+        _constraintAddr = NativeHelpers.AllocateAlignedMemory(sizeof(IKConstraint), 16);
+    }
+
+    public void SolveIK(hkaPose* pose, ushort startBone, ushort endBone, Vector3 target, int iterations)
+    {
+        hkaCCDSolver* ccdSolver = (hkaCCDSolver*)_solverAddr.Aligned;
+        _ccdSolverCtr(ccdSolver, iterations, 1f);
+
+        IKConstraint* constraint = (IKConstraint*)_constraintAddr.Aligned;
+        constraint->StartBone = startBone;
+        constraint->EndBone = endBone;
+        constraint->Target.X = target.X;
+        constraint->Target.Y = target.Y;
+        constraint->Target.Z = target.Z;
+
+        var constraints = new hkArray<IKConstraint>
+        {
+            Length = 1,
+            CapacityAndFlags = 1,
+            Data = constraint
+        };
+
+        byte notSure = 0;
+        _ccdSolverSolve(ccdSolver, &notSure, &constraints, pose);
+    }
+
+    public void Dispose()
+    {
+        NativeHelpers.FreeAlignedMemory(_solverAddr);
+        NativeHelpers.FreeAlignedMemory(_constraintAddr);
+    }
+
+    [StructLayout(LayoutKind.Explicit, Size = 0x18)]
+    private struct hkaCCDSolver
+    {
+        [FieldOffset(0x0)] public nint vtbl;
+        [FieldOffset(0x10)] public uint Iterations;
+        [FieldOffset(0x14)] public float Gain;
+    }
+
+    [StructLayout(LayoutKind.Explicit, Size = 0x2)]
+    private struct IKConstraint
+    {
+        [FieldOffset(0x0)] public ushort StartBone;
+        [FieldOffset(0x2)] public ushort EndBone;
+        [FieldOffset(0x10)] public hkVector4f Target;
+    }
+
+}

Brio/Game/Posing/PoseImporter.cs

@@ -16,7 +16,7 @@ public void ApplyBone(Bone bone, BonePoseInfo poseInfo)
             {
                 if (poseFile.Bones.TryGetValue(bone.Name, out var fileBone))
                 {
-                    poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, PoseMirrorMode.None, true);
+                    poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Default, PoseMirrorMode.None, true);
                 }
             }
         }
@@ -28,7 +28,7 @@ public void ApplyBone(Bone bone, BonePoseInfo poseInfo)
             {
                 if (poseFile.MainHand.TryGetValue(bone.Name, out var fileBone))
                 {
-                    poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, PoseMirrorMode.None, true);
+                    poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Default, PoseMirrorMode.None, true);
                 }
             }
         }
@@ -40,7 +40,7 @@ public void ApplyBone(Bone bone, BonePoseInfo poseInfo)
             {
                 if (poseFile.OffHand.TryGetValue(bone.Name, out var fileBone))
                 {
-                    poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, PoseMirrorMode.None, true);
+                    poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Default, PoseMirrorMode.None, true);
                 }
             }
         }

Brio/Game/Posing/PoseInfo.cs

@@ -1,5 +1,6 @@
 using Brio.Core;
 using Brio.Game.Posing.Skeletons;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 
@@ -11,25 +12,44 @@ internal class PoseInfo
 
     public BonePoseInfo GetPoseInfo(BonePoseInfoId id)
     {
-        if (_poses.TryGetValue(id, out var pose))
+        if(_poses.TryGetValue(id, out var pose))
             return pose;
 
         return _poses[id] = new BonePoseInfo(id, this);
     }
 
     public bool IsOveridden => _poses.Any(x => x.Value.HasStacks);
 
+    public bool HasIKStacks => _poses.Any(x => x.Value.Stacks.Any(s => s.IKInfo.Enabled));
+
+    public Dictionary<string, int> StackCounts
+    {
+        get
+        {
+            Dictionary<string, int> counts = [];
+            foreach(var pose in _poses)
+            {
+                if(pose.Value.Stacks.Count > 0)
+                    counts[pose.Key.BoneName] = pose.Value.Stacks.Count;
+            }
+            return counts;
+        }
+    }
+
     public unsafe BonePoseInfo GetPoseInfo(Bone bone, PoseInfoSlot slot = PoseInfoSlot.Character) => GetPoseInfo(new BonePoseInfoId(bone.Name, bone.PartialId, slot));
 
     public void Clear()
     {
-        _poses.Clear();
+        foreach(var pose in _poses)
+        {
+            pose.Value.ClearStacks();
+        }
     }
 
     public PoseInfo Clone()
     {
         var clone = new PoseInfo();
-        foreach (var pose in _poses)
+        foreach(var pose in _poses)
         {
             clone._poses.Add(pose.Key, pose.Value.Clone(clone));
         }
@@ -48,6 +68,8 @@ internal class BonePoseInfo(BonePoseInfoId id, PoseInfo parent)
 
     public TransformComponents DefaultPropagation { get; set; } = TransformComponents.Position | TransformComponents.Rotation;
 
+    public BoneIKInfo DefaultIK { get; set; } = BoneIKInfo.Default;
+
     public IReadOnlyList<BonePoseTransformInfo> Stacks => _stacks;
 
     public PoseMirrorMode MirrorMode { get; set; } = PoseMirrorMode.None;
@@ -56,30 +78,36 @@ internal class BonePoseInfo(BonePoseInfoId id, PoseInfo parent)
 
     public bool HasStacks => _stacks.Any();
 
-    public void Apply(Transform transform, Transform? original = null, TransformComponents? propagation = null, TransformComponents applyTo = TransformComponents.All, PoseMirrorMode? mirrorMode = null, bool forceNewStack = false)
+    public void Apply(Transform transform, Transform? original = null, TransformComponents? propagation = null, TransformComponents applyTo = TransformComponents.All, BoneIKInfo? ikInfo = null, PoseMirrorMode? mirrorMode = null, bool forceNewStack = false)
     {
         var prop = propagation ?? DefaultPropagation;
+        ikInfo ??= DefaultIK;
         var calc = original.HasValue ? transform.CalculateDiff(original.Value) : transform;
-        var transformIndex = GetTransformIndex(prop, forceNewStack);
+        var transformIndex = GetTransformIndex(prop, ikInfo.Value, forceNewStack);
         mirrorMode ??= MirrorMode;
 
         var existing = _stacks[transformIndex].Transform;
 
         calc.Filter(applyTo);
-        if (Transform.Identity.IsApproximatelySame(calc + existing))
+        if(Transform.Identity.IsApproximatelySame(calc + existing))
             return;
 
-        if (mirrorMode == PoseMirrorMode.Copy)
+        if(mirrorMode == PoseMirrorMode.Copy)
         {
-            GetMirrorBone()?.Apply(calc, null, prop, applyTo, PoseMirrorMode.None, forceNewStack);
+            GetMirrorBone()?.Apply(calc, null, prop, applyTo, ikInfo.Value, PoseMirrorMode.None, forceNewStack);
         }
-        else if (mirrorMode == PoseMirrorMode.Mirror)
+        else if(mirrorMode == PoseMirrorMode.Mirror)
         {
             var inverted = calc.Inverted();
-            GetMirrorBone()?.Apply(inverted, null, prop, applyTo, PoseMirrorMode.None, forceNewStack);
+            GetMirrorBone()?.Apply(inverted, null, prop, applyTo, ikInfo.Value, PoseMirrorMode.None, forceNewStack);
         }
 
-        _stacks[transformIndex] = new(prop, _stacks[transformIndex].Transform + calc);
+        _stacks[transformIndex] = new(prop, ikInfo.Value, _stacks[transformIndex].Transform + calc);
+    }
+
+    public void ClearStacks()
+    {
+        _stacks.Clear();
     }
 
     public BonePoseInfo Clone(PoseInfo parent)
@@ -98,28 +126,28 @@ public BonePoseInfo Clone(PoseInfo parent)
     public BonePoseInfo? GetMirrorBone()
     {
         var mirror = Id.GetMirrorBone();
-        if (mirror.HasValue)
+        if(mirror.HasValue)
             return Parent.GetPoseInfo(mirror.Value);
 
         return null;
     }
 
-    private int GetTransformIndex(TransformComponents components, bool forceNewStack)
+    private int GetTransformIndex(TransformComponents components, BoneIKInfo ikInfo, bool forceNewStack)
     {
-        if (_stacks.Count == 0)
+        if(_stacks.Count == 0)
         {
-            _stacks.Add(new(components, Transform.Identity));
+            _stacks.Add(new(components, ikInfo, Transform.Identity));
             return 0;
         }
 
-        if (!forceNewStack)
+        if(!forceNewStack)
         {
             var entry = _stacks[^1];
-            if (entry.PropagateComponents == components)
+            if(entry.PropagateComponents == components && entry.IKInfo.Equals(ikInfo))
                 return _stacks.Count - 1;
         }
 
-        _stacks.Add(new(components, Transform.Identity));
+        _stacks.Add(new(components, ikInfo, Transform.Identity));
         return _stacks.Count - 1;
     }
 }
@@ -146,13 +174,13 @@ internal record struct BonePoseInfoId(string BoneName, int Partial, PoseInfoSlot
 
     public readonly BonePoseInfoId? GetMirrorBone()
     {
-        if (BoneName.EndsWith("_r"))
+        if(BoneName.EndsWith("_r"))
         {
             var mirrorName = BoneName.Substring(0, BoneName.Length - 2) + "_l";
             return new BonePoseInfoId(mirrorName, Partial, Slot);
         }
 
-        if (BoneName.EndsWith("_l"))
+        if(BoneName.EndsWith("_l"))
         {
             var mirrorName = BoneName.Substring(0, BoneName.Length - 2) + "_r";
             return new BonePoseInfoId(mirrorName, Partial, Slot);
@@ -161,4 +189,25 @@ internal record struct BonePoseInfoId(string BoneName, int Partial, PoseInfoSlot
         return null;
     }
 }
-internal record struct BonePoseTransformInfo(TransformComponents PropagateComponents, Transform Transform);
+internal record struct BonePoseTransformInfo(TransformComponents PropagateComponents, BoneIKInfo IKInfo, Transform Transform);
+
+internal struct BoneIKInfo
+{
+    public bool Enabled = false;
+    public int Depth = 3;
+    public int Iterations = 8;
+    public bool EnforceConstraints = true;
+
+    public static readonly BoneIKInfo Default = new();
+
+
+    public BoneIKInfo()
+    {
+
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Enabled, Depth, Iterations);
+    }
+}