Local Space Transforms

CustomizePlus/Armatures/Data/Interop.cs

@@ -0,0 +1,59 @@
+using System;
+using System.Numerics;
+using System.Runtime.InteropServices;
+
+using FFXIVClientStructs.FFXIV.Client.System.Memory;
+using FFXIVClientStructs.Havok.Common.Base.Math.Matrix;
+using FFXIVClientStructs.Havok.Common.Base.Math.QsTransform;
+
+namespace CustomizePlus.Armatures.Data;
+internal static class InteropAlloc
+{
+    // Allocations
+    private static IntPtr MatrixAlloc;
+
+    // Access
+    internal unsafe static Matrix4x4* Matrix; // Align to 16-byte boundary
+    internal unsafe static Matrix4x4 GetMatrix(hkQsTransformf* transform)
+    {
+        transform->get4x4ColumnMajor((float*)Matrix);
+        return *Matrix;
+    }
+    internal unsafe static void SetMatrix(hkQsTransformf* transform, Matrix4x4 matrix)
+    {
+        *Matrix = matrix;
+        transform->set((hkMatrix4f*)Matrix);
+    }
+
+    // Init & disspose
+    public unsafe static void Init()
+    {
+        // Allocate space for our matrix to be aligned on a 16-byte boundary.
+        // This is required due to ffxiv's use of the MOVAPS instruction.
+        // Thanks to Fayti1703 for helping with debugging and coming up with this fix.
+        MatrixAlloc = Marshal.AllocHGlobal(sizeof(float) * 16 + 16);
+        Matrix = (Matrix4x4*)(16 * ((long)(MatrixAlloc + 15) / 16));
+    }
+    public static void Dispose()
+    {
+        Marshal.FreeHGlobal(MatrixAlloc);
+    }
+}
+
+internal class GameAlloc<T> : IDisposable where T : unmanaged
+{
+    private bool Disposed;
+
+    internal readonly nint Address;
+    internal unsafe T* Data => (T*)Address;
+
+    internal unsafe GameAlloc(ulong align = 16)
+        => Address = (nint)IMemorySpace.GetDefaultSpace()->Malloc<T>(align);
+
+    public unsafe void Dispose()
+    {
+        if (Disposed) return;
+        IMemorySpace.Free(Data); // Free our allocated memory.
+        Disposed = true;
+    }
+}

CustomizePlus/Armatures/Data/ModelBone.cs

@@ -1,11 +1,19 @@
 using System;
 using System.Collections.Generic;
 using System.Linq;
+using System.Numerics;
+using CustomizePlus.Configuration.Data.Version3;
 using CustomizePlus.Core.Data;
+using CustomizePlus.Core.Extensions;
 using CustomizePlus.Templates.Data;
 using FFXIVClientStructs.FFXIV.Client.Graphics.Scene;
 using FFXIVClientStructs.Havok;
+using FFXIVClientStructs.Havok.Animation.Rig;
 using FFXIVClientStructs.Havok.Common.Base.Math.QsTransform;
+using OtterGui.Text.EndObjects;
+using Penumbra.GameData;
+using static CustomizePlus.Anamnesis.Data.PoseFile;
+using static FFXIVClientStructs.Havok.Animation.Rig.hkaPose;
 
 namespace CustomizePlus.Armatures.Data;
 
@@ -39,6 +47,15 @@ public enum PoseType
     /// A model bone may have zero children.
     /// </summary>
     public IEnumerable<ModelBone> ChildBones => _childPartialIndices.Zip(_childBoneIndices, (x, y) => MasterArmature[x, y]);
+    public IEnumerable<ModelBone> GetDescendants()
+    {
+        var list = ChildBones.ToList();
+        for (var i = 0; i < list.Count; i++)
+        {
+            list.AddRange(list[i].ChildBones.ToList());
+        }
+        return list;
+    }
     private List<int> _childPartialIndices = new();
     private List<int> _childBoneIndices = new();
 
@@ -201,6 +218,8 @@ public hkQsTransformf GetGameTransform(CharacterBase* cBase, PoseType refFrame)
 
         if (targetPose == null) return Constants.NullTransform;
 
+        if (BoneIndex >= targetPose->Skeleton->Bones.Length) return Constants.NullTransform;
+
         return refFrame switch
         {
             PoseType.Local => targetPose->LocalPose[BoneIndex],
@@ -210,6 +229,29 @@ public hkQsTransformf GetGameTransform(CharacterBase* cBase, PoseType refFrame)
         };
     }
 
+    public hkQsTransformf* GetGameTransformAccess(CharacterBase* cBase, PoseType refFrame)
+    {
+
+        var skelly = cBase->Skeleton;
+        var pSkelly = skelly->PartialSkeletons[PartialSkeletonIndex];
+        var targetPose = pSkelly.GetHavokPose(Constants.TruePoseIndex);
+        //hkaPose* targetPose = cBase->Skeleton->PartialSkeletons[PartialSkeletonIndex].GetHavokPose(Constants.TruePoseIndex);
+
+        if (targetPose == null) return null;
+        const PropagateOrNot DO_NOT_PROPAGATE = 0;
+
+        // It's really gonna crash without it, skeleton changes aren't getting picked up fast enough
+        if (BoneIndex >= targetPose->Skeleton->Bones.Length) return null;
+
+        return refFrame switch
+        {
+            PoseType.Local => targetPose->AccessBoneLocalSpace(BoneIndex),
+            PoseType.Model => targetPose->AccessBoneModelSpace(BoneIndex, DO_NOT_PROPAGATE),
+            _ => null
+            //TODO properly implement the other options
+        }; ;
+    }
+
     private void SetGameTransform(CharacterBase* cBase, hkQsTransformf transform, PoseType refFrame)
     {
         SetGameTransform(cBase, transform, PartialSkeletonIndex, BoneIndex, refFrame);
@@ -245,19 +287,81 @@ private static void SetGameTransform(CharacterBase* cBase, hkQsTransformf transf
     /// Apply this model bone's associated transformation to its in-game sibling within
     /// the skeleton of the given character base.
     /// </summary>
+
     public void ApplyModelTransform(CharacterBase* cBase)
     {
         if (!IsActive)
             return;
 
         if (cBase != null
-            && CustomizedTransform.IsEdited()
-            && GetGameTransform(cBase, PoseType.Model) is hkQsTransformf gameTransform
-            && !gameTransform.Equals(Constants.NullTransform)
-            && CustomizedTransform.ModifyExistingTransform(gameTransform) is hkQsTransformf modTransform
-            && !modTransform.Equals(Constants.NullTransform))
+            && CustomizedTransform != null && CustomizedTransform.IsEdited())
+        {   
+            var gameTransformAccess = GetGameTransformAccess(cBase, PoseType.Model);
+            if (gameTransformAccess != null)
+            {
+                // Transforms before any modifications
+                var initialPos = gameTransformAccess->Translation.ToVector3();
+                var initialRot = gameTransformAccess->Rotation.ToQuaternion();
+                var initialScale = gameTransformAccess->Scale.ToVector3();
+
+                var modTransform = CustomizedTransform.ModifyExistingTransform(*gameTransformAccess);
+                SetGameTransform(cBase, modTransform, PoseType.Model);
+
+                // Getting it again, because SetGameTransform replaced the previous Havok transform object
+                var access2 = GetGameTransformAccess(cBase, PoseType.Model);
+
+                var propagateTranslation = CustomizedTransform.propagateTranslation &&
+                                           !CustomizedTransform.Translation.Equals(Vector3.Zero);
+                var propagateRotation = CustomizedTransform.propagateRotation &&
+                                           !CustomizedTransform.Rotation.Equals(Vector3.Zero);
+                var propagateScale = CustomizedTransform.propagateScale &&
+                                           !CustomizedTransform.Scaling.Equals(Vector3.One);
+
+                if (propagateTranslation || propagateRotation || propagateScale)
+                {
+                    // Plugin.Logger.Debug($">>> Start propagating from {BoneName}, Translation: {propagateTranslation} Rotation: {propagateRotation} Scale: {propagateScale}");
+                    PropagateChildren(cBase, access2, initialPos, initialRot, initialScale, propagateTranslation, propagateRotation, propagateScale);
+                }
+            }
+        }
+    }
+
+    public unsafe void PropagateChildren(CharacterBase* cBase, hkQsTransformf* transform, Vector3 initialPos, Quaternion initialRot, Vector3 initialScale, bool propagateTranslation, bool propagateRotation, bool propagateScale, bool includePartials = true)
+    {
+        // Bone parenting
+        // Adapted from Anamnesis Studio code shared by Yuki - thank you!
+
+        // Original Parent Bone position after it had its offsets applied
+        var sourcePos = transform->Translation.ToVector3();
+
+        var deltaRot = transform->Rotation.ToQuaternion() / initialRot;
+        var deltaPos = sourcePos - initialPos;
+        var deltaScale = transform->Scale.ToVector3() / initialScale;
+
+        foreach (var child in GetDescendants())
         {
-            SetGameTransform(cBase, modTransform, PoseType.Model);
+            // Plugin.Logger.Debug($"Propagating to {child.BoneName}...");
+            var access = child.GetGameTransformAccess(cBase, PoseType.Model);
+            if (access != null)
+            {
+
+                var offset = access->Translation.ToVector3() - sourcePos;
+
+                var matrix = InteropAlloc.GetMatrix(access);
+                if (propagateScale)
+                {
+                    var scaleMatrix = Matrix4x4.CreateScale(deltaScale, Vector3.Zero);
+                    matrix *= scaleMatrix;
+                    offset = Vector3.Transform(offset, scaleMatrix);
+                }
+                if (propagateRotation)
+                {
+                    matrix *= Matrix4x4.CreateFromQuaternion(deltaRot);
+                    offset = Vector3.Transform(offset, deltaRot);
+                }
+                matrix.Translation = deltaPos + sourcePos + offset;
+                InteropAlloc.SetMatrix(access, matrix);
+            }
         }
     }
 

CustomizePlus/Core/Data/BoneTransform.cs

@@ -57,6 +57,9 @@ public Vector3 Scaling
         get => _scaling;
         set => _scaling = ClampVector(value);
     }
+    public bool propagateTranslation = false;
+    public bool propagateRotation = false;
+    public bool propagateScale = false;
 
     [OnDeserialized]
     internal void OnDeserialized(StreamingContext context)
@@ -80,23 +83,29 @@ public BoneTransform DeepCopy()
         {
             Translation = Translation,
             Rotation = Rotation,
-            Scaling = Scaling
+            Scaling = Scaling,
+            propagateTranslation = propagateTranslation,
+            propagateRotation = propagateRotation,
+            propagateScale = propagateScale
         };
     }
 
-    public void UpdateAttribute(BoneAttribute which, Vector3 newValue)
+    public void UpdateAttribute(BoneAttribute which, Vector3 newValue, bool shouldPropagate)
     {
         if (which == BoneAttribute.Position)
         {
             Translation = newValue;
+            propagateTranslation = shouldPropagate;
         }
         else if (which == BoneAttribute.Rotation)
         {
             Rotation = newValue;
+            propagateRotation = shouldPropagate;
         }
         else
         {
             Scaling = newValue;
+            propagateScale = shouldPropagate;
         }
     }
 
@@ -105,6 +114,9 @@ public void UpdateToMatch(BoneTransform newValues)
         Translation = newValues.Translation;
         Rotation = newValues.Rotation;
         Scaling = newValues.Scaling;
+        propagateTranslation = newValues.propagateTranslation;
+        propagateRotation = newValues.propagateRotation;
+        propagateScale = newValues.propagateScale;
     }
 
     /// <summary>
@@ -117,7 +129,10 @@ public BoneTransform GetStandardReflection()
         {
             Translation = new Vector3(Translation.X, Translation.Y, -1 * Translation.Z),
             Rotation = new Vector3(-1 * Rotation.X, -1 * Rotation.Y, Rotation.Z),
-            Scaling = Scaling
+            Scaling = Scaling,
+            propagateTranslation = propagateTranslation,
+            propagateRotation = propagateRotation,
+            propagateScale = propagateScale
         };
     }
 
@@ -131,7 +146,10 @@ public BoneTransform GetSpecialReflection()
         {
             Translation = new Vector3(Translation.X, -1 * Translation.Y, Translation.Z),
             Rotation = new Vector3(Rotation.X, -1 * Rotation.Y, -1 * Rotation.Z),
-            Scaling = Scaling
+            Scaling = Scaling,
+            propagateTranslation = propagateTranslation,
+            propagateRotation = propagateRotation,
+            propagateScale = propagateScale
         };
     }
 

CustomizePlus/Core/Extensions/VectorExtensions.cs

@@ -29,6 +29,8 @@ private static bool IsApproximately(float a, float b, float errorMargin)
         return d < errorMargin;
     }
 
+    public static Vector3 ToVector3(this hkVector4f vec) => new Vector3(vec.X, vec.Y, vec.Z);
+
     public static Quaternion ToQuaternion(this Vector3 rotation)
     {
         return Quaternion.CreateFromYawPitchRoll(

CustomizePlus/Plugin.cs

@@ -9,6 +9,7 @@
 using OtterGui.Log;
 using OtterGui.Services;
 using Penumbra.GameData.Actors;
+using CustomizePlus.Armatures.Data;
 
 namespace CustomizePlus;
 
@@ -23,6 +24,7 @@ public Plugin(IDalamudPluginInterface pluginInterface)
         try
         {
             ECommonsMain.Init(pluginInterface, this);
+            InteropAlloc.Init();
 
             _services = ServiceManagerBuilder.CreateProvider(pluginInterface, Logger);