Skip to content

Commit

Permalink
TwoJoint solver
Browse files Browse the repository at this point in the history
  • Loading branch information
@AsgardXIV
AsgardXIV committed Jan 4, 2024
1 parent 7bf9122 commit 9e4015b
Show file tree
Hide file tree
Showing 5 changed files with 185 additions and 47 deletions.
119 changes: 90 additions & 29 deletions Brio/Game/Posing/IKService.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,54 +3,91 @@
using Dalamud.Game;
using FFXIVClientStructs.Havok;
using System;
using System.Linq;
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;
delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<CCDIKConstraint>*, hkaPose*, byte*> _ccdSolverSolve;
delegate* unmanaged<byte*, TwoJointIKSetup*, hkaPose*, byte*> _twoJointSolverSolve;

private (nint Aligned, nint Unaligned) _solverAddr;
private (nint Aligned, nint Unaligned) _constraintAddr;
private (nint Aligned, nint Unaligned) _ccdConstraintCtrAddr;
private (nint Aligned, nint Unaligned) _twoJointSetupAddr;

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");

_ccdSolverSolve = (delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<CCDIKConstraint>*, hkaPose*, byte*>)scanner.ScanText("E8 ?? ?? ?? ?? 8B 45 ?? 48 8B 75");
_twoJointSolverSolve = (delegate* unmanaged<byte*, TwoJointIKSetup*, hkaPose*, byte*>)scanner.ScanText("E8 ?? ?? ?? ?? 0F 28 55 ?? 41 0F 28 D8");

_solverAddr = NativeHelpers.AllocateAlignedMemory(sizeof(hkaCCDSolver), 16);
_constraintAddr = NativeHelpers.AllocateAlignedMemory(sizeof(IKConstraint), 16);
_ccdConstraintCtrAddr = NativeHelpers.AllocateAlignedMemory(sizeof(CCDIKConstraint), 16);
_twoJointSetupAddr = NativeHelpers.AllocateAlignedMemory(sizeof(TwoJointIKSetup), 16);

TwoJointIKSetup* setup = (TwoJointIKSetup*)_twoJointSetupAddr.Aligned;
*setup = new TwoJointIKSetup();
}

public void SolveIK(hkaPose* pose, ushort startBone, ushort endBone, Vector3 target, int iterations)
public void SolveIK(hkaPose* pose, BoneIKInfo ikInfo, Bone bone, Vector3 target)
{
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);
ikInfo.SolverOptions.Switch(
ccd =>
{
var boneList = bone.GetBonesToDepth(ccd.Depth, true);
if(boneList.Count <= 1)
return;

var startBone = (short)boneList.Last().Index;
var endBone = (short)boneList.First().Index;

hkaCCDSolver* ccdSolver = (hkaCCDSolver*)_solverAddr.Aligned;
_ccdSolverCtr(ccdSolver, ccd.Iterations, 1f);

CCDIKConstraint* constraint = (CCDIKConstraint*)_ccdConstraintCtrAddr.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<CCDIKConstraint>
{
Length = 1,
CapacityAndFlags = 1,
Data = constraint
};

byte notSure = 0;
_ccdSolverSolve(ccdSolver, &notSure, &constraints, pose);
},
twoJoint =>
{
var boneList = bone.GetBonesToDepth(twoJoint.FirstBone, true);

if(boneList.Count < twoJoint.FirstBone)
return;

TwoJointIKSetup* setup = (TwoJointIKSetup*)_twoJointSetupAddr.Aligned;
setup->FirstJointIdx = (short)boneList[twoJoint.FirstBone].Index;
setup->SecondJointIdx = (short)boneList[twoJoint.SecondBone].Index;
setup->EndBoneIdx = (short)boneList[twoJoint.EndBone].Index;
setup->EndTargetMS = new Vector4(target, 0);
setup->HingeAxisLS = new Vector4(twoJoint.RotationAxis, 0);

byte notSure = 0;
_twoJointSolverSolve(&notSure, setup, pose);
}
);
}

public void Dispose()
{
NativeHelpers.FreeAlignedMemory(_solverAddr);
NativeHelpers.FreeAlignedMemory(_constraintAddr);
NativeHelpers.FreeAlignedMemory(_ccdConstraintCtrAddr);
}

[StructLayout(LayoutKind.Explicit, Size = 0x18)]
Expand All @@ -62,11 +99,35 @@ private struct hkaCCDSolver
}

[StructLayout(LayoutKind.Explicit, Size = 0x2)]
private struct IKConstraint
private struct CCDIKConstraint
{
[FieldOffset(0x0)] public ushort StartBone;
[FieldOffset(0x2)] public ushort EndBone;
[FieldOffset(0x0)] public short StartBone;
[FieldOffset(0x2)] public short EndBone;
[FieldOffset(0x10)] public hkVector4f Target;
}

[StructLayout(LayoutKind.Explicit, Size = 0x82)]
private struct TwoJointIKSetup
{
[FieldOffset(0x00)] public short FirstJointIdx = -1;
[FieldOffset(0x02)] public short SecondJointIdx = -1;
[FieldOffset(0x04)] public short EndBoneIdx = -1;
[FieldOffset(0x06)] public short FirstJointTwistIdx = -1;
[FieldOffset(0x08)] public short SecondJointTwistIdx = -1;
[FieldOffset(0x10)] public Vector4 HingeAxisLS = Vector4.Zero;
[FieldOffset(0x20)] public float CosineMaxHingeAngle = -1f;
[FieldOffset(0x24)] public float CosineMinHingeAngle = 1f;
[FieldOffset(0x28)] public float FirstJointIkGain = 1f;
[FieldOffset(0x2C)] public float SecondJointIkGain = 1f;
[FieldOffset(0x30)] public float EndJointIkGain = 1f;
[FieldOffset(0x40)] public Vector4 EndTargetMS = Vector4.Zero;
[FieldOffset(0x50)] public Quaternion EndTargetRotationMS = Quaternion.Identity;
[FieldOffset(0x60)] public Vector4 EndBoneOffsetLS = Vector4.Zero;
[FieldOffset(0x70)] public Quaternion EndBoneRotationOffsetLS = Quaternion.Identity;
[FieldOffset(0x80)] public bool EnforceEndPosition = true;
[FieldOffset(0x81)] public bool EnforceEndRotation = false;

public TwoJointIKSetup() { }
}

}
6 changes: 3 additions & 3 deletions Brio/Game/Posing/PoseImporter.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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, BoneIKInfo.Default, PoseMirrorMode.None, true);
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Disabled, PoseMirrorMode.None, true);
}
}
}
Expand All @@ -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, BoneIKInfo.Default, PoseMirrorMode.None, true);
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Disabled, PoseMirrorMode.None, true);
}
}
}
Expand All @@ -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, BoneIKInfo.Default, PoseMirrorMode.None, true);
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Disabled, PoseMirrorMode.None, true);
}
}
}
Expand Down
60 changes: 54 additions & 6 deletions Brio/Game/Posing/PoseInfo.cs
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,10 @@
using Brio.Core;
using Brio.Game.Posing.Skeletons;
using OneOf;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;

namespace Brio.Game.Posing;

Expand Down Expand Up @@ -68,7 +70,7 @@ internal class BonePoseInfo(BonePoseInfoId id, PoseInfo parent)

public TransformComponents DefaultPropagation { get; set; } = TransformComponents.Position | TransformComponents.Rotation;

public BoneIKInfo DefaultIK { get; set; } = BoneIKInfo.Default;
public BoneIKInfo DefaultIK { get; set; } = BoneIKInfo.CalculateDefault(id.BoneName);

public IReadOnlyList<BonePoseTransformInfo> Stacks => _stacks;

Expand Down Expand Up @@ -194,20 +196,66 @@ internal record struct BonePoseTransformInfo(TransformComponents PropagateCompon
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 OneOf<CCDOptions, TwoJointOptions> SolverOptions = new CCDOptions();

public readonly static BoneIKInfo Disabled = new();

public static bool CanUseJoint(string boneName) => boneName.StartsWith("j_te") || boneName.StartsWith("j_asi_d");

public static BoneIKInfo CalculateDefault(string boneName, bool allowJoint = true)
{
var result = new BoneIKInfo();

if(allowJoint && CanUseJoint(boneName))
{
if(boneName.StartsWith("j_te"))
{
var options = new TwoJointOptions()
{
FirstBone = 2,
SecondBone = 1,
EndBone = 0,
RotationAxis = Vector3.UnitZ
};
result.SolverOptions = options;
}

if(boneName.StartsWith("j_asi_d"))
{
var options = new TwoJointOptions()
{
FirstBone = 3,
SecondBone = 1,
EndBone = 0,
RotationAxis = -Vector3.UnitZ
};
result.SolverOptions = options;
}
}

return result;
}


public BoneIKInfo()
{

}

public override int GetHashCode()
public struct CCDOptions()
{
public int Depth = 3;
public int Iterations = 8;
}

public struct TwoJointOptions()
{
return HashCode.Combine(Enabled, Depth, Iterations);
public int FirstBone = -1;
public int SecondBone = -1;
public int EndBone = -1;
public Vector3 RotationAxis = Vector3.Zero;
}
}
4 changes: 1 addition & 3 deletions Brio/Game/Posing/SkeletonService.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,16 +270,14 @@ private void ApplySnapshot(hkaPose* pose, Bone bone, BonePoseTransformInfo info)
var trans = info.Transform;
trans.Filter(info.PropagateComponents);


// Position
bool prop = info.PropagateComponents.HasFlag(TransformComponents.Position);
var modelSpace = pose->AccessBoneModelSpace(boneId, prop ? PropagateOrNot.Propagate : PropagateOrNot.DontPropagate);
temp = modelSpace;
temp.Position += info.Transform.Position;
if(info.IKInfo.Enabled)
{
var impactedBones = bone.GetBonesToDepth(info.IKInfo.Depth, true);
_ikService.SolveIK(pose, (ushort)impactedBones.Last().Index, (ushort)impactedBones.First().Index, temp.Position, info.IKInfo.Iterations);
_ikService.SolveIK(pose, info.IKInfo, bone, temp.Position);

if(!info.IKInfo.EnforceConstraints)
{
Expand Down
43 changes: 37 additions & 6 deletions Brio/UI/Controls/Editors/BoneIKEditor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,46 @@ public static void Draw(BonePoseInfo poseInfo, PosingCapability posing)
didChange |= true;
}

if(ImGui.SliderInt("Depth", ref ik.Depth, 1, 20))
{
didChange |= true;
}

if(ImGui.SliderInt("Iterations", ref ik.Iterations, 1, 20))
string solverType = ik.SolverOptions.Match(_ => "CCD", _ => "Two Joint");
using(var combo = ImRaii.Combo("Solver", solverType))
{
didChange |= true;
if(combo.Success)
{
if(ImGui.Selectable("CCD"))
{
ik.SolverOptions = BoneIKInfo.CalculateDefault(poseInfo.Name, false).SolverOptions;
didChange |= true;
}

if(BoneIKInfo.CanUseJoint(poseInfo.Name))
{
if(ImGui.Selectable("Two Joint"))
{
ik.SolverOptions = BoneIKInfo.CalculateDefault(poseInfo.Name, true).SolverOptions;
didChange |= true;
}
}
}
}

ik.SolverOptions.Switch(
ccd =>
{
if(ImGui.SliderInt("Depth", ref ccd.Depth, 1, 20))
{
didChange |= true;
}

if(ImGui.SliderInt("Iterations", ref ccd.Iterations, 1, 20))
{
didChange |= true;
}
},
twoJoint =>
{
}
);
}

if(didChange)
Expand Down

0 comments on commit 9e4015b

Please sign in to comment.