Skip to content

Commit 9e4015b

Browse files
AsgardXIVAsgardXIV
committed
TwoJoint solver
1 parent 7bf9122 commit 9e4015b

File tree

5 files changed

+185
-47
lines changed

5 files changed

+185
-47
lines changed

‎Brio/Game/Posing/IKService.cs

+90-29
Original file line numberDiff line numberDiff line change
@@ -3,54 +3,91 @@
33
using Dalamud.Game;
44
using FFXIVClientStructs.Havok;
55
using System;
6+
using System.Linq;
67
using System.Numerics;
78
using System.Runtime.InteropServices;
89

910
namespace Brio.Game.Posing;
1011
internal unsafe class IKService : IDisposable
1112
{
1213
delegate* unmanaged<hkaCCDSolver*, int, float, void> _ccdSolverCtr;
13-
delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<IKConstraint>*, hkaPose*, byte*> _ccdSolverSolve;
14+
delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<CCDIKConstraint>*, hkaPose*, byte*> _ccdSolverSolve;
15+
delegate* unmanaged<byte*, TwoJointIKSetup*, hkaPose*, byte*> _twoJointSolverSolve;
1416

1517
private (nint Aligned, nint Unaligned) _solverAddr;
16-
private (nint Aligned, nint Unaligned) _constraintAddr;
18+
private (nint Aligned, nint Unaligned) _ccdConstraintCtrAddr;
19+
private (nint Aligned, nint Unaligned) _twoJointSetupAddr;
1720

1821
public IKService(ISigScanner scanner)
1922
{
2023
_ccdSolverCtr = (delegate* unmanaged<hkaCCDSolver*, int, float, void>)scanner.ScanText("E8 ?? ?? ?? ?? BA ?? ?? ?? ?? 48 C7 43");
21-
_ccdSolverSolve = (delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<IKConstraint>*, hkaPose*, byte*>)scanner.ScanText("E8 ?? ?? ?? ?? 8B 45 ?? 48 8B 75");
22-
24+
_ccdSolverSolve = (delegate* unmanaged<hkaCCDSolver*, byte*, hkArray<CCDIKConstraint>*, hkaPose*, byte*>)scanner.ScanText("E8 ?? ?? ?? ?? 8B 45 ?? 48 8B 75");
25+
_twoJointSolverSolve = (delegate* unmanaged<byte*, TwoJointIKSetup*, hkaPose*, byte*>)scanner.ScanText("E8 ?? ?? ?? ?? 0F 28 55 ?? 41 0F 28 D8");
26+
2327
_solverAddr = NativeHelpers.AllocateAlignedMemory(sizeof(hkaCCDSolver), 16);
24-
_constraintAddr = NativeHelpers.AllocateAlignedMemory(sizeof(IKConstraint), 16);
28+
_ccdConstraintCtrAddr = NativeHelpers.AllocateAlignedMemory(sizeof(CCDIKConstraint), 16);
29+
_twoJointSetupAddr = NativeHelpers.AllocateAlignedMemory(sizeof(TwoJointIKSetup), 16);
30+
31+
TwoJointIKSetup* setup = (TwoJointIKSetup*)_twoJointSetupAddr.Aligned;
32+
*setup = new TwoJointIKSetup();
2533
}
2634

27-
public void SolveIK(hkaPose* pose, ushort startBone, ushort endBone, Vector3 target, int iterations)
35+
public void SolveIK(hkaPose* pose, BoneIKInfo ikInfo, Bone bone, Vector3 target)
2836
{
29-
hkaCCDSolver* ccdSolver = (hkaCCDSolver*)_solverAddr.Aligned;
30-
_ccdSolverCtr(ccdSolver, iterations, 1f);
31-
32-
IKConstraint* constraint = (IKConstraint*)_constraintAddr.Aligned;
33-
constraint->StartBone = startBone;
34-
constraint->EndBone = endBone;
35-
constraint->Target.X = target.X;
36-
constraint->Target.Y = target.Y;
37-
constraint->Target.Z = target.Z;
38-
39-
var constraints = new hkArray<IKConstraint>
40-
{
41-
Length = 1,
42-
CapacityAndFlags = 1,
43-
Data = constraint
44-
};
45-
46-
byte notSure = 0;
47-
_ccdSolverSolve(ccdSolver, &notSure, &constraints, pose);
37+
ikInfo.SolverOptions.Switch(
38+
ccd =>
39+
{
40+
var boneList = bone.GetBonesToDepth(ccd.Depth, true);
41+
if(boneList.Count <= 1)
42+
return;
43+
44+
var startBone = (short)boneList.Last().Index;
45+
var endBone = (short)boneList.First().Index;
46+
47+
hkaCCDSolver* ccdSolver = (hkaCCDSolver*)_solverAddr.Aligned;
48+
_ccdSolverCtr(ccdSolver, ccd.Iterations, 1f);
49+
50+
CCDIKConstraint* constraint = (CCDIKConstraint*)_ccdConstraintCtrAddr.Aligned;
51+
constraint->StartBone = startBone;
52+
constraint->EndBone = endBone;
53+
constraint->Target.X = target.X;
54+
constraint->Target.Y = target.Y;
55+
constraint->Target.Z = target.Z;
56+
57+
var constraints = new hkArray<CCDIKConstraint>
58+
{
59+
Length = 1,
60+
CapacityAndFlags = 1,
61+
Data = constraint
62+
};
63+
64+
byte notSure = 0;
65+
_ccdSolverSolve(ccdSolver, &notSure, &constraints, pose);
66+
},
67+
twoJoint =>
68+
{
69+
var boneList = bone.GetBonesToDepth(twoJoint.FirstBone, true);
70+
71+
if(boneList.Count < twoJoint.FirstBone)
72+
return;
73+
74+
TwoJointIKSetup* setup = (TwoJointIKSetup*)_twoJointSetupAddr.Aligned;
75+
setup->FirstJointIdx = (short)boneList[twoJoint.FirstBone].Index;
76+
setup->SecondJointIdx = (short)boneList[twoJoint.SecondBone].Index;
77+
setup->EndBoneIdx = (short)boneList[twoJoint.EndBone].Index;
78+
setup->EndTargetMS = new Vector4(target, 0);
79+
setup->HingeAxisLS = new Vector4(twoJoint.RotationAxis, 0);
80+
81+
byte notSure = 0;
82+
_twoJointSolverSolve(&notSure, setup, pose);
83+
}
84+
);
4885
}
4986

5087
public void Dispose()
5188
{
5289
NativeHelpers.FreeAlignedMemory(_solverAddr);
53-
NativeHelpers.FreeAlignedMemory(_constraintAddr);
90+
NativeHelpers.FreeAlignedMemory(_ccdConstraintCtrAddr);
5491
}
5592

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

64101
[StructLayout(LayoutKind.Explicit, Size = 0x2)]
65-
private struct IKConstraint
102+
private struct CCDIKConstraint
66103
{
67-
[FieldOffset(0x0)] public ushort StartBone;
68-
[FieldOffset(0x2)] public ushort EndBone;
104+
[FieldOffset(0x0)] public short StartBone;
105+
[FieldOffset(0x2)] public short EndBone;
69106
[FieldOffset(0x10)] public hkVector4f Target;
70107
}
71108

109+
[StructLayout(LayoutKind.Explicit, Size = 0x82)]
110+
private struct TwoJointIKSetup
111+
{
112+
[FieldOffset(0x00)] public short FirstJointIdx = -1;
113+
[FieldOffset(0x02)] public short SecondJointIdx = -1;
114+
[FieldOffset(0x04)] public short EndBoneIdx = -1;
115+
[FieldOffset(0x06)] public short FirstJointTwistIdx = -1;
116+
[FieldOffset(0x08)] public short SecondJointTwistIdx = -1;
117+
[FieldOffset(0x10)] public Vector4 HingeAxisLS = Vector4.Zero;
118+
[FieldOffset(0x20)] public float CosineMaxHingeAngle = -1f;
119+
[FieldOffset(0x24)] public float CosineMinHingeAngle = 1f;
120+
[FieldOffset(0x28)] public float FirstJointIkGain = 1f;
121+
[FieldOffset(0x2C)] public float SecondJointIkGain = 1f;
122+
[FieldOffset(0x30)] public float EndJointIkGain = 1f;
123+
[FieldOffset(0x40)] public Vector4 EndTargetMS = Vector4.Zero;
124+
[FieldOffset(0x50)] public Quaternion EndTargetRotationMS = Quaternion.Identity;
125+
[FieldOffset(0x60)] public Vector4 EndBoneOffsetLS = Vector4.Zero;
126+
[FieldOffset(0x70)] public Quaternion EndBoneRotationOffsetLS = Quaternion.Identity;
127+
[FieldOffset(0x80)] public bool EnforceEndPosition = true;
128+
[FieldOffset(0x81)] public bool EnforceEndRotation = false;
129+
130+
public TwoJointIKSetup() { }
131+
}
132+
72133
}

‎Brio/Game/Posing/PoseImporter.cs

+3-3
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ public void ApplyBone(Bone bone, BonePoseInfo poseInfo)
1616
{
1717
if (poseFile.Bones.TryGetValue(bone.Name, out var fileBone))
1818
{
19-
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Default, PoseMirrorMode.None, true);
19+
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Disabled, PoseMirrorMode.None, true);
2020
}
2121
}
2222
}
@@ -28,7 +28,7 @@ public void ApplyBone(Bone bone, BonePoseInfo poseInfo)
2828
{
2929
if (poseFile.MainHand.TryGetValue(bone.Name, out var fileBone))
3030
{
31-
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Default, PoseMirrorMode.None, true);
31+
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Disabled, PoseMirrorMode.None, true);
3232
}
3333
}
3434
}
@@ -40,7 +40,7 @@ public void ApplyBone(Bone bone, BonePoseInfo poseInfo)
4040
{
4141
if (poseFile.OffHand.TryGetValue(bone.Name, out var fileBone))
4242
{
43-
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Default, PoseMirrorMode.None, true);
43+
poseInfo.Apply(fileBone, bone.LastTransform, TransformComponents.All, options.TransformComponents, BoneIKInfo.Disabled, PoseMirrorMode.None, true);
4444
}
4545
}
4646
}

‎Brio/Game/Posing/PoseInfo.cs

+54-6
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,10 @@
11
using Brio.Core;
22
using Brio.Game.Posing.Skeletons;
3+
using OneOf;
34
using System;
45
using System.Collections.Generic;
56
using System.Linq;
7+
using System.Numerics;
68

79
namespace Brio.Game.Posing;
810

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

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

71-
public BoneIKInfo DefaultIK { get; set; } = BoneIKInfo.Default;
73+
public BoneIKInfo DefaultIK { get; set; } = BoneIKInfo.CalculateDefault(id.BoneName);
7274

7375
public IReadOnlyList<BonePoseTransformInfo> Stacks => _stacks;
7476

@@ -194,20 +196,66 @@ internal record struct BonePoseTransformInfo(TransformComponents PropagateCompon
194196
internal struct BoneIKInfo
195197
{
196198
public bool Enabled = false;
197-
public int Depth = 3;
198-
public int Iterations = 8;
199+
199200
public bool EnforceConstraints = true;
200201

201-
public static readonly BoneIKInfo Default = new();
202+
public OneOf<CCDOptions, TwoJointOptions> SolverOptions = new CCDOptions();
203+
204+
public readonly static BoneIKInfo Disabled = new();
205+
206+
public static bool CanUseJoint(string boneName) => boneName.StartsWith("j_te") || boneName.StartsWith("j_asi_d");
207+
208+
public static BoneIKInfo CalculateDefault(string boneName, bool allowJoint = true)
209+
{
210+
var result = new BoneIKInfo();
211+
212+
if(allowJoint && CanUseJoint(boneName))
213+
{
214+
if(boneName.StartsWith("j_te"))
215+
{
216+
var options = new TwoJointOptions()
217+
{
218+
FirstBone = 2,
219+
SecondBone = 1,
220+
EndBone = 0,
221+
RotationAxis = Vector3.UnitZ
222+
};
223+
result.SolverOptions = options;
224+
}
225+
226+
if(boneName.StartsWith("j_asi_d"))
227+
{
228+
var options = new TwoJointOptions()
229+
{
230+
FirstBone = 3,
231+
SecondBone = 1,
232+
EndBone = 0,
233+
RotationAxis = -Vector3.UnitZ
234+
};
235+
result.SolverOptions = options;
236+
}
237+
}
238+
239+
return result;
240+
}
202241

203242

204243
public BoneIKInfo()
205244
{
206245

207246
}
208247

209-
public override int GetHashCode()
248+
public struct CCDOptions()
249+
{
250+
public int Depth = 3;
251+
public int Iterations = 8;
252+
}
253+
254+
public struct TwoJointOptions()
210255
{
211-
return HashCode.Combine(Enabled, Depth, Iterations);
256+
public int FirstBone = -1;
257+
public int SecondBone = -1;
258+
public int EndBone = -1;
259+
public Vector3 RotationAxis = Vector3.Zero;
212260
}
213261
}

‎Brio/Game/Posing/SkeletonService.cs

+1-3
Original file line numberDiff line numberDiff line change
@@ -270,16 +270,14 @@ private void ApplySnapshot(hkaPose* pose, Bone bone, BonePoseTransformInfo info)
270270
var trans = info.Transform;
271271
trans.Filter(info.PropagateComponents);
272272

273-
274273
// Position
275274
bool prop = info.PropagateComponents.HasFlag(TransformComponents.Position);
276275
var modelSpace = pose->AccessBoneModelSpace(boneId, prop ? PropagateOrNot.Propagate : PropagateOrNot.DontPropagate);
277276
temp = modelSpace;
278277
temp.Position += info.Transform.Position;
279278
if(info.IKInfo.Enabled)
280279
{
281-
var impactedBones = bone.GetBonesToDepth(info.IKInfo.Depth, true);
282-
_ikService.SolveIK(pose, (ushort)impactedBones.Last().Index, (ushort)impactedBones.First().Index, temp.Position, info.IKInfo.Iterations);
280+
_ikService.SolveIK(pose, info.IKInfo, bone, temp.Position);
283281

284282
if(!info.IKInfo.EnforceConstraints)
285283
{

‎Brio/UI/Controls/Editors/BoneIKEditor.cs

+37-6
Original file line numberDiff line numberDiff line change
@@ -32,15 +32,46 @@ public static void Draw(BonePoseInfo poseInfo, PosingCapability posing)
3232
didChange |= true;
3333
}
3434

35-
if(ImGui.SliderInt("Depth", ref ik.Depth, 1, 20))
36-
{
37-
didChange |= true;
38-
}
3935

40-
if(ImGui.SliderInt("Iterations", ref ik.Iterations, 1, 20))
36+
string solverType = ik.SolverOptions.Match(_ => "CCD", _ => "Two Joint");
37+
using(var combo = ImRaii.Combo("Solver", solverType))
4138
{
42-
didChange |= true;
39+
if(combo.Success)
40+
{
41+
if(ImGui.Selectable("CCD"))
42+
{
43+
ik.SolverOptions = BoneIKInfo.CalculateDefault(poseInfo.Name, false).SolverOptions;
44+
didChange |= true;
45+
}
46+
47+
if(BoneIKInfo.CanUseJoint(poseInfo.Name))
48+
{
49+
if(ImGui.Selectable("Two Joint"))
50+
{
51+
ik.SolverOptions = BoneIKInfo.CalculateDefault(poseInfo.Name, true).SolverOptions;
52+
didChange |= true;
53+
}
54+
}
55+
}
4356
}
57+
58+
ik.SolverOptions.Switch(
59+
ccd =>
60+
{
61+
if(ImGui.SliderInt("Depth", ref ccd.Depth, 1, 20))
62+
{
63+
didChange |= true;
64+
}
65+
66+
if(ImGui.SliderInt("Iterations", ref ccd.Iterations, 1, 20))
67+
{
68+
didChange |= true;
69+
}
70+
},
71+
twoJoint =>
72+
{
73+
}
74+
);
4475
}
4576

4677
if(didChange)

0 commit comments

Comments
 (0)