From 02649963ba3e602020aa99e39c623d935f06744b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 16:43:27 +0300 Subject: [PATCH 01/14] Port studio IK chain, rule and lock data --- Source.Common/Studio.cs | 222 ++++++++++++++++++++++++++++ Source.Common/StudioVirtualModel.cs | 26 +++- 2 files changed, 247 insertions(+), 1 deletion(-) diff --git a/Source.Common/Studio.cs b/Source.Common/Studio.cs index 2a87b152f..ba53f4d9e 100644 --- a/Source.Common/Studio.cs +++ b/Source.Common/Studio.cs @@ -99,6 +99,13 @@ public static string ProduceASCIIString(ref string? cache, in Span data) { public const int STUDIO_VERSION = 48; + public const int IK_SELF = 1; + public const int IK_WORLD = 2; + public const int IK_GROUND = 3; + public const int IK_RELEASE = 4; + public const int IK_ATTACHMENT = 5; + public const int IK_UNLATCH = 6; + public const int MAXSTUDIOTRIANGLES = 65536; public const int MAXSTUDIOVERTS = 65536; public const int MAXSTUDIOFLEXVERTS = 10000; @@ -736,6 +743,49 @@ public MStudioEyeball(Memory data) { /// /// analog of mstudiomodel_t /// +public class MStudioIKLink +{ + public const int SIZEOF = 28; + public static MStudioIKLink FACTORY(object caller, Memory data) => new(data); + + public int Bone; + public Vector3 KneeDir; + + public MStudioIKLink(Memory data) { + SpanBinaryReader br = new(data.Span); + br.Read(out Bone); + br.Read(out KneeDir); + } +} + +public class MStudioIKChain +{ + public const int SIZEOF = 16; + public static MStudioIKChain FACTORY(object caller, Memory data) => new(data); + + public Memory Data; + + public int NameIndex; + string? nameCache; + public string Name() => Studio.ProduceASCIIString(ref nameCache, Data.Span[NameIndex..]); + + public int LinkType; + public int NumLinks; + public int LinkIndex; + MStudioIKLink[]? linkCache; + public MStudioIKLink Link(int i) + => Studio.ProduceArrayIdx(this, ref linkCache, NumLinks, LinkIndex, i, MStudioIKLink.SIZEOF, Data, MStudioIKLink.FACTORY); + + public MStudioIKChain(Memory data) { + Data = data; + SpanBinaryReader br = new(data.Span); + br.Read(out NameIndex); + br.Read(out LinkType); + br.Read(out NumLinks); + br.Read(out LinkIndex); + } +} + public class MStudioModel { public const int SIZEOF = 148; @@ -1128,6 +1178,132 @@ public unsafe ref Vector48 Pos() } } +public struct MStudioIKError +{ + public const int SIZEOF = 28; + + public Vector3 Pos; + public Quaternion Q; +} + +public class MStudioCompressedIKError +{ + public const int SIZEOF = 36; + + public Memory Data; + public InlineArray6 Scale; + public InlineArray6 Offset; + + public MStudioCompressedIKError(Memory data) { + Data = data; + SpanBinaryReader br = new(data.Span); + br.ReadInto(Scale); + br.ReadInto(Offset); + } + + public Span Animvalue(int i) { + if (Offset[i] > 0) + return Data.Span[Offset[i]..].Cast(); + return null; + } +} + +public class MStudioIKRule +{ + public const int SIZEOF = 152; + public static MStudioIKRule FACTORY(object caller, Memory data) => new(data); + + public Memory Data; + + public int Index; + + public int Type; + public int Chain; + + public int Bone; + + public int Slot; + public float Height; + public float Radius; + public float Floor; + public Vector3 Pos; + public Quaternion Q; + + public int CompressedIKErrorIndex; + MStudioCompressedIKError? compressedErrorCache; + public MStudioCompressedIKError CompressedError() => compressedErrorCache ??= new(Data[CompressedIKErrorIndex..]); + + public int StartFrame; + public int IKErrorIndex; + public ReadOnlySpan Error(int i) { + if (IKErrorIndex != 0) + return Data.Span[(IKErrorIndex + (i - StartFrame) * MStudioIKError.SIZEOF)..].Cast(); + return null; + } + + public float Start; + public float Peak; + public float Tail; + public float End; + + public float Contact; + public float Drop; + public float Top; + + public int AttachmentNameIndex; + string? attachmentCache; + public string Attachment() => Studio.ProduceASCIIString(ref attachmentCache, Data.Span[AttachmentNameIndex..]); + + public MStudioIKRule(Memory data) { + Data = data; + SpanBinaryReader br = new(data.Span); + + br.Read(out Index); + br.Read(out Type); + br.Read(out Chain); + br.Read(out Bone); + br.Read(out Slot); + br.Read(out Height); + br.Read(out Radius); + br.Read(out Floor); + br.Read(out Pos); + br.Read(out Q); + br.Read(out CompressedIKErrorIndex); + br.Advance(1); + br.Read(out StartFrame); + br.Read(out IKErrorIndex); + br.Read(out Start); + br.Read(out Peak); + br.Read(out Tail); + br.Read(out End); + br.Advance(1); + br.Read(out Contact); + br.Read(out Drop); + br.Read(out Top); + br.Advance(3); + br.Read(out AttachmentNameIndex); + } +} + +public class MStudioIKLock +{ + public const int SIZEOF = 32; + public static MStudioIKLock FACTORY(object caller, Memory data) => new(data); + + public int Chain; + public float PosWeight; + public float LocalQWeight; + public int Flags; + + public MStudioIKLock(Memory data) { + SpanBinaryReader br = new(data.Span); + br.Read(out Chain); + br.Read(out PosWeight); + br.Read(out LocalQWeight); + br.Read(out Flags); + } +} + public class MStudioLocalHierarchy { public const int SIZEOF = 48; // Static offset from MSVC stats (mstudiobone_t size 48, alignment 4) @@ -1324,6 +1500,27 @@ MStudioAnim CacheOffBlockIndex(int block, int index, Memory data) { public int NumIKRules; public int IKRuleIndex; public int AnimBlockIKRuleIndex; + MStudioIKRule?[]? ikRuleCache; + public MStudioIKRule? IKRule(int i) { + ikRuleCache ??= new MStudioIKRule?[NumIKRules]; + if (ikRuleCache[i] != null) + return ikRuleCache[i]; + + if (IKRuleIndex != 0) + return ikRuleCache[i] = MStudioIKRule.FACTORY(this, Data[(IKRuleIndex + i * MStudioIKRule.SIZEOF)..]); + else if (AnimBlockIKRuleIndex != 0) { + if (nAnimBlock == 0) + return ikRuleCache[i] = MStudioIKRule.FACTORY(this, Data[(AnimBlockIKRuleIndex + i * MStudioIKRule.SIZEOF)..]); + else { + Memory animBlocks = Studiohdr().GetAnimBlock(nAnimBlock); + + if (!animBlocks.IsEmpty) + return ikRuleCache[i] = MStudioIKRule.FACTORY(this, animBlocks[(AnimBlockIKRuleIndex + i * MStudioIKRule.SIZEOF)..]); + } + } + + return null; + } public int NumLocalHierarchy; public int LocalHierarchyIndex; @@ -1750,6 +1947,9 @@ public Span Boneweights(int i) public int NumIKLocks; public int IKLockIndex; + MStudioIKLock[]? ikLockCache; + public MStudioIKLock IKLock(int i) + => Studio.ProduceArrayIdx(this, ref ikLockCache, NumIKLocks, IKLockIndex, i, MStudioIKLock.SIZEOF, Data, MStudioIKLock.FACTORY); public int KeyValueIndex; public int KeyValueSize; @@ -2018,6 +2218,22 @@ public void SetEventListVersion(int version) { public int NumFlexRules() => studioHdr!.NumFlexRules; public MStudioFlexRule FlexRule(int i) => studioHdr!.FlexRule(i); public int GetNumIKChains() => studioHdr!.NumIKChains; + public MStudioIKChain IKChain(int i) => studioHdr!.IKChain(i); + + public int GetNumIKAutoplayLocks() { + if (vModel == null) + return studioHdr!.NumLocalIKAutoplayLocks; + + return vModel.IKLock.Count; + } + + public MStudioIKLock IKAutoplayLock(int i) { + if (vModel == null) + return this.studioHdr!.LocalIKAutoplayLock(i); + + StudioHeader studioHdr = GroupStudioHdr(vModel.IKLock[i].Group)!; + return studioHdr.LocalIKAutoplayLock(vModel.IKLock[i].Index); + } public float MaxEyeDeflection() => studioHdr!.MaxEyeDeflection(); public void RunFlexRules(ReadOnlySpan src, Span dest) { @@ -2871,6 +3087,9 @@ public MStudioFlexRule FlexRule(int i) public int NumIKChains; public int IKChainIndex; + MStudioIKChain[]? ikChainCache; + public MStudioIKChain IKChain(int i) + => Studio.ProduceArrayIdx(this, ref ikChainCache, NumIKChains, IKChainIndex, i, MStudioIKChain.SIZEOF, Data, MStudioIKChain.FACTORY); public int NumMouths; public int MouthIndex; @@ -2892,6 +3111,9 @@ public MStudioPoseParamDesc LocalPoseParameter(int i) public int NumLocalIKAutoplayLocks; public int LocalIKAutoplayLockIndex; + MStudioIKLock[]? localIKAutoplayLockCache; + public MStudioIKLock LocalIKAutoplayLock(int i) + => Studio.ProduceArrayIdx(this, ref localIKAutoplayLockCache, NumLocalIKAutoplayLocks, LocalIKAutoplayLockIndex, i, MStudioIKLock.SIZEOF, Data, MStudioIKLock.FACTORY); public float Mass; public int Contents; diff --git a/Source.Common/StudioVirtualModel.cs b/Source.Common/StudioVirtualModel.cs index eb025c5a3..3a973efb8 100644 --- a/Source.Common/StudioVirtualModel.cs +++ b/Source.Common/StudioVirtualModel.cs @@ -344,7 +344,31 @@ public void AppendNodes(int group, StudioHeader studioHdr) { } public void AppendIKLocks(int group, StudioHeader studioHDR) { - // todo: IK studiomodel + lock (Lock) { + int numCheck = IKLock.Count; + + BeginTemporaryCopyList(IKLock, out var iklock); + + int j, k; + + for (j = 0; j < studioHDR.NumLocalIKAutoplayLocks; j++) { + int chain1 = studioHDR.LocalIKAutoplayLock(j).Chain; + for (k = 0; k < numCheck; k++) { + int chain2 = Group[iklock[k].Group].GetStudioHdr()!.LocalIKAutoplayLock(iklock[k].Index).Chain; + + if (chain1 == chain2) + break; + } + if (k == numCheck) { + VirtualGeneric tmp; + tmp.Group = group; + tmp.Index = j; + k = iklock.Count; iklock.Add(tmp); + } + } + + EndTemporaryCopyList(IKLock, iklock); + } } struct HandleAndHeader_t From 2bd9cea2fb44f9465c3da884b4df170682108f1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 17:00:14 +0300 Subject: [PATCH 02/14] Fix animdesc zero frame span and count read as int --- Source.Common/Studio.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Source.Common/Studio.cs b/Source.Common/Studio.cs index ba53f4d9e..a46b7fb4d 100644 --- a/Source.Common/Studio.cs +++ b/Source.Common/Studio.cs @@ -1533,8 +1533,8 @@ MStudioAnim CacheOffBlockIndex(int block, int index, Memory data) { public MStudioAnimSections Section(int i) => Studio.ProduceArrayIdx(this, ref sections, (NumFrames / SectionFrames) + 2, SectionIndex, i, MStudioAnimSections.SIZEOF, Data, MStudioAnimSections.FACTORY); - public int ZeroFrameSpan; - public int ZeroFrameCount; + public short ZeroFrameSpan; + public short ZeroFrameCount; public int ZeroFrameIndex; public Span ZeroFrameData() { if (ZeroFrameIndex != 0) { From 261e08e076c38532da88ce9fe4119e76a4c2d26f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 17:39:47 +0300 Subject: [PATCH 03/14] Port studio two bone IK solver --- Source.Common/BoneSetup.cs | 2 +- Source.Common/BoneSetupIK.cs | 166 +++++++++++++++++++++++++++++++++++ 2 files changed, 167 insertions(+), 1 deletion(-) create mode 100644 Source.Common/BoneSetupIK.cs diff --git a/Source.Common/BoneSetup.cs b/Source.Common/BoneSetup.cs index 494c046fa..6a1676af7 100644 --- a/Source.Common/BoneSetup.cs +++ b/Source.Common/BoneSetup.cs @@ -156,7 +156,7 @@ private Span CachedToStudio() { public const int BoneCache_SIZE = 24; } -public ref struct BoneSetup +public ref partial struct BoneSetup { public static readonly ConVar anim_3wayblend = new("anim_3wayblend", "1", FCvar.Replicated, "Toggle the 3-way animation blending code."); diff --git a/Source.Common/BoneSetupIK.cs b/Source.Common/BoneSetupIK.cs new file mode 100644 index 000000000..630cc0e3f --- /dev/null +++ b/Source.Common/BoneSetupIK.cs @@ -0,0 +1,166 @@ +using Source.Common.Mathematics; + +using System.Numerics; + +namespace Source.Common; + +public struct IKSolver +{ + static float FindD(float a, float b, float c) => (c + (a * a - b * b) / c) / 2; + static float FindE(float a, float d) => MathF.Sqrt(a * a - d * d); + + Vector3 MfwdX, MfwdY, MfwdZ; + Vector3 MinvX, MinvY, MinvZ; + + public bool Solve(float A, float B, in Vector3 P, in Vector3 D, out Vector3 Q) { + DefineM(P, D); + Vector3 R = Rot(MinvX, MinvY, MinvZ, P); + float r = Length(R); + float d = FindD(A, B, r); + float e = FindE(A, d); + Vector3 S = new(d, e, 0); + Q = Rot(MfwdX, MfwdY, MfwdZ, S); + return d > (r - B) && d < A; + } + + void DefineM(in Vector3 P, in Vector3 D) { + MinvX = P; + Normalize(ref MinvX); + + float dDOTx = Dot(D, MinvX); + MinvY = new(D.X - dDOTx * MinvX.X, D.Y - dDOTx * MinvX.Y, D.Z - dDOTx * MinvX.Z); + Normalize(ref MinvY); + + MinvZ = Cross(MinvX, MinvY); + + MfwdX = new(MinvX.X, MinvY.X, MinvZ.X); + MfwdY = new(MinvX.Y, MinvY.Y, MinvZ.Y); + MfwdZ = new(MinvX.Z, MinvY.Z, MinvZ.Z); + } + + static float Dot(in Vector3 a, in Vector3 b) => a.X * b.X + a.Y * b.Y + a.Z * b.Z; + + static float Length(in Vector3 v) => MathF.Sqrt(Dot(v, v)); + + static void Normalize(ref Vector3 v) { + float norm = Length(v); + v.X /= norm; + v.Y /= norm; + v.Z /= norm; + } + + static Vector3 Cross(in Vector3 a, in Vector3 b) => new(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X); + + static Vector3 Rot(in Vector3 m0, in Vector3 m1, in Vector3 m2, in Vector3 src) => new(Dot(m0, src), Dot(m1, src), Dot(m2, src)); +} + +public ref partial struct BoneSetup +{ + public static bool Studio_SolveIK(MStudioIKChain ikchain, in Vector3 targetFoot, Span boneToWorld) { + if (ikchain.Link(0).KneeDir.LengthSquared() > 0.0) { + Vector3 tmp = ikchain.Link(0).KneeDir; + MathLib.VectorRotate(tmp, boneToWorld[ikchain.Link(0).Bone], out Vector3 targetKneeDir); + MathLib.MatrixPosition(boneToWorld[ikchain.Link(1).Bone], out Vector3 targetKneePos); + return Studio_SolveIK(ikchain.Link(0).Bone, ikchain.Link(1).Bone, ikchain.Link(2).Bone, targetFoot, targetKneePos, targetKneeDir, boneToWorld); + } + else + return Studio_SolveIK(ikchain.Link(0).Bone, ikchain.Link(1).Bone, ikchain.Link(2).Bone, targetFoot, boneToWorld); + } + + const double KNEEMAX_EPSILON = 0.9998; + + public static bool Studio_SolveIK(int thigh, int knee, int foot, in Vector3 targetFoot, Span boneToWorld) { + MathLib.MatrixPosition(boneToWorld[thigh], out Vector3 worldThigh); + MathLib.MatrixPosition(boneToWorld[knee], out Vector3 worldKnee); + MathLib.MatrixPosition(boneToWorld[foot], out Vector3 worldFoot); + + Vector3 ikFoot, ikKnee; + + ikFoot = targetFoot - worldThigh; + ikKnee = worldKnee - worldThigh; + + float l1 = (worldKnee - worldThigh).Length(); + float l2 = (worldFoot - worldKnee).Length(); + float l3 = (worldFoot - worldThigh).Length(); + + if (l3 > (l1 + l2) * KNEEMAX_EPSILON) + return false; + + Vector3 ikHalf = (worldFoot - worldThigh) * (l1 / l3); + + Vector3 ikKneeDir = ikKnee - ikHalf; + MathLib.VectorNormalize(ref ikKneeDir); + + return Studio_SolveIK(thigh, knee, foot, targetFoot, worldKnee, ikKneeDir, boneToWorld); + } + + public static void Studio_AlignIKMatrix(ref Matrix3x4 mat, in Vector3 alignTo) { + Vector3 tmp1, tmp2, tmp3; + + tmp1 = alignTo; + MathLib.VectorNormalize(ref tmp1); + MathLib.MatrixSetColumn(tmp1, 0, ref mat); + + MathLib.MatrixGetColumn(mat, 2, out tmp3); + tmp2 = Vector3.Cross(tmp3, tmp1); + MathLib.VectorNormalize(ref tmp2); + MathLib.MatrixSetColumn(tmp2, 1, ref mat); + + tmp3 = Vector3.Cross(tmp1, tmp2); + MathLib.MatrixSetColumn(tmp3, 2, ref mat); + } + + public static bool Studio_SolveIK(int thigh, int knee, int foot, in Vector3 targetFoot, in Vector3 targetKneePos, in Vector3 targetKneeDir, Span boneToWorld) { + MathLib.MatrixPosition(boneToWorld[thigh], out Vector3 worldThigh); + MathLib.MatrixPosition(boneToWorld[knee], out Vector3 worldKnee); + MathLib.MatrixPosition(boneToWorld[foot], out Vector3 worldFoot); + + Vector3 ikFoot, ikTargetKnee, ikKnee; + + ikFoot = targetFoot - worldThigh; + ikKnee = targetKneePos - worldThigh; + + float l1 = (worldKnee - worldThigh).Length(); + float l2 = (worldFoot - worldKnee).Length(); + + float d = (targetFoot - worldThigh).Length() - Math.Min(l1, l2); + d = Math.Max(l1 + l2, d); + d = d * 100; + + ikTargetKnee = ikKnee + targetKneeDir * d; + + if (ikFoot.Length() > (l1 + l2) * KNEEMAX_EPSILON) { + MathLib.VectorNormalize(ref ikFoot); + ikFoot = ikFoot * (float)((l1 + l2) * KNEEMAX_EPSILON); + } + + float minDist = (float)Math.Max(MathF.Abs(l1 - l2) * 1.15, Math.Min(l1, l2) * 0.15); + if (ikFoot.Length() < minDist) { + ikFoot = worldFoot - worldThigh; + MathLib.VectorNormalize(ref ikFoot); + ikFoot = ikFoot * minDist; + } + + IKSolver ik = default; + if (ik.Solve(l1, l2, ikFoot, ikTargetKnee, out ikKnee)) { + ref Matrix3x4 mWorldThigh = ref boneToWorld[thigh]; + ref Matrix3x4 mWorldKnee = ref boneToWorld[knee]; + ref Matrix3x4 mWorldFoot = ref boneToWorld[foot]; + + Studio_AlignIKMatrix(ref mWorldThigh, ikKnee); + Studio_AlignIKMatrix(ref mWorldKnee, ikFoot - ikKnee); + + mWorldKnee[0, 3] = ikKnee.X + worldThigh.X; + mWorldKnee[1, 3] = ikKnee.Y + worldThigh.Y; + mWorldKnee[2, 3] = ikKnee.Z + worldThigh.Z; + + mWorldFoot[0, 3] = ikFoot.X + worldThigh.X; + mWorldFoot[1, 3] = ikFoot.Y + worldThigh.Y; + mWorldFoot[2, 3] = ikFoot.Z + worldThigh.Z; + + return true; + } + else + return false; + } +} From 269e6a1bdb2c864aad0bf085429546bca20086cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 18:30:22 +0300 Subject: [PATCH 04/14] Port studio IK rule weight and error evaluation --- Source.Common/BoneSetupIK.cs | 316 +++++++++++++++++++++++++++ Source.Common/Mathematics/MathLib.cs | 16 ++ Source.Common/Studio.cs | 8 + 3 files changed, 340 insertions(+) diff --git a/Source.Common/BoneSetupIK.cs b/Source.Common/BoneSetupIK.cs index 630cc0e3f..7a60fcded 100644 --- a/Source.Common/BoneSetupIK.cs +++ b/Source.Common/BoneSetupIK.cs @@ -164,3 +164,319 @@ public static bool Studio_SolveIK(int thigh, int knee, int foot, in Vector3 targ return false; } } + +public struct IKContextIKRule +{ + public int Index; + + public int Type; + public int Chain; + + public int Bone; + + public int Slot; + public float Height; + public float Radius; + public float Floor; + public Vector3 Pos; + public Quaternion Q; + + public float Start; + public float Peak; + public float Tail; + public float End; + + public float Top; + public float Drop; + + public float Commit; + public float Release; + + public float Weight; + public float RuleWeight; + public float Latched; + public string? Label; + + public Vector3 KneeDir; + public Vector3 KneePos; +} + +public ref partial struct BoneSetup +{ + public static void QuaternionAccumulate(in Quaternion p, float s, in Quaternion q, out Quaternion qt) { + MathLib.QuaternionAlign(p, q, out Quaternion q2); + + Quaternion result; + result.X = p.X + s * q2.X; + result.Y = p.Y + s * q2.Y; + result.Z = p.Z + s * q2.Z; + result.W = p.W + s * q2.W; + qt = result; + } + + static void CalcDecompressedAnimation(MStudioCompressedIKError compressed, int frame, TimeUnit_t fraq, out Vector3 pos, out Quaternion q) { + if (fraq > 0.0001f) { + Vector3 p1, p2; + ExtractAnimValue(frame, compressed.Animvalue(0), compressed.Scale[0], out p1.X, out p2.X); + ExtractAnimValue(frame, compressed.Animvalue(1), compressed.Scale[1], out p1.Y, out p2.Y); + ExtractAnimValue(frame, compressed.Animvalue(2), compressed.Scale[2], out p1.Z, out p2.Z); + pos = p1 * (float)(1 - fraq) + p2 * (float)fraq; + + Quaternion q1, q2; + RadianEuler angle1, angle2; + ExtractAnimValue(frame, compressed.Animvalue(3), compressed.Scale[3], out angle1.X, out angle2.X); + ExtractAnimValue(frame, compressed.Animvalue(4), compressed.Scale[4], out angle1.Y, out angle2.Y); + ExtractAnimValue(frame, compressed.Animvalue(5), compressed.Scale[5], out angle1.Z, out angle2.Z); + + if (angle1.X != angle2.X || angle1.Y != angle2.Y || angle1.Z != angle2.Z) { + MathLib.AngleQuaternion(angle1, out q1); + MathLib.AngleQuaternion(angle2, out q2); + MathLib.QuaternionBlend(q1, q2, (float)fraq, out q); + } + else + MathLib.AngleQuaternion(angle1, out q); + } + else { + ExtractAnimValue(frame, compressed.Animvalue(0), compressed.Scale[0], out pos.X); + ExtractAnimValue(frame, compressed.Animvalue(1), compressed.Scale[1], out pos.Y); + ExtractAnimValue(frame, compressed.Animvalue(2), compressed.Scale[2], out pos.Z); + + RadianEuler angle; + ExtractAnimValue(frame, compressed.Animvalue(3), compressed.Scale[3], out angle.X); + ExtractAnimValue(frame, compressed.Animvalue(4), compressed.Scale[4], out angle.Y); + ExtractAnimValue(frame, compressed.Animvalue(5), compressed.Scale[5], out angle.Z); + + MathLib.AngleQuaternion(angle, out q); + } + } + + public static float Studio_IKRuleWeight(MStudioIKRule ikRule, MStudioAnimDesc anim, TimeUnit_t cycle, out int frame, out TimeUnit_t fraq) { + if (ikRule.End > 1.0f && cycle < ikRule.Start) + cycle = cycle + 1.0f; + + TimeUnit_t value = 0.0; + fraq = (anim.NumFrames - 1) * (cycle - ikRule.Start) + ikRule.StartFrame; + frame = (int)fraq; + fraq = fraq - frame; + + if (cycle < ikRule.Start) { + frame = ikRule.StartFrame; + fraq = 0.0; + return 0.0f; + } + else if (cycle < ikRule.Peak) + value = (cycle - ikRule.Start) / (ikRule.Peak - ikRule.Start); + else if (cycle < ikRule.Tail) + return 1.0f; + else if (cycle < ikRule.End) + value = 1.0 - ((cycle - ikRule.Tail) / (ikRule.End - ikRule.Tail)); + else { + fraq = (anim.NumFrames - 1) * (ikRule.End - ikRule.Start) + ikRule.StartFrame; + frame = (int)fraq; + fraq = fraq - frame; + } + return (float)MathLib.SimpleSpline(value); + } + + public static float Studio_IKRuleWeight(ref IKContextIKRule ikRule, TimeUnit_t cycle) { + if (ikRule.End > 1.0f && cycle < ikRule.Start) + cycle = cycle + 1.0f; + + TimeUnit_t value = 0.0; + if (cycle < ikRule.Start) + return 0.0f; + else if (cycle < ikRule.Peak) + value = (cycle - ikRule.Start) / (ikRule.Peak - ikRule.Start); + else if (cycle < ikRule.Tail) + return 1.0f; + else if (cycle < ikRule.End) + value = 1.0 - ((cycle - ikRule.Tail) / (ikRule.End - ikRule.Tail)); + + return (float)(3.0 * value * value - 2.0 * value * value * value); + } + + public static bool Studio_IKShouldLatch(ref IKContextIKRule ikRule, TimeUnit_t cycle) { + if (ikRule.End > 1.0f && cycle < ikRule.Start) + cycle = cycle + 1.0f; + + if (cycle < ikRule.Peak) + return false; + else if (cycle < ikRule.End) + return true; + + return false; + } + + public static float Studio_IKTail(ref IKContextIKRule ikRule, TimeUnit_t cycle) { + if (ikRule.End > 1.0f && cycle < ikRule.Start) + cycle = cycle + 1.0f; + + if (cycle <= ikRule.Tail) + return 0.0f; + else if (cycle < ikRule.End) + return (float)((cycle - ikRule.Tail) / (ikRule.End - ikRule.Tail)); + + return 0.0f; + } + + public static bool Studio_IKAnimationError(StudioHdr studioHdr, MStudioIKRule rule, MStudioAnimDesc anim, TimeUnit_t cycle, out Vector3 pos, out Quaternion q, out float weight) { + weight = Studio_IKRuleWeight(rule, anim, cycle, out int frame, out TimeUnit_t fraq); + Assert(fraq >= 0.0 && fraq < 1.0); + Assert(weight >= 0.0f && weight <= 1.0f); + + weight = Math.Clamp(weight, 0.0f, 1.0f); + + pos = default; + q = default; + + if (rule.Type != Studio.IK_GROUND && weight < 0.0001) + return false; + + ReadOnlySpan error = rule.Error(frame); + if (!error.IsEmpty) { + if (fraq < 0.001) { + q = error[0].Q; + pos = error[0].Pos; + } + else { + MathLib.QuaternionBlend(error[0].Q, error[1].Q, (float)fraq, out q); + pos = error[0].Pos * (float)(1.0 - fraq) + error[1].Pos * (float)fraq; + } + return true; + } + + MStudioCompressedIKError compressed = rule.CompressedError(); + CalcDecompressedAnimation(compressed, frame - rule.StartFrame, fraq, out pos, out q); + return true; + } + + public static bool Studio_IKSequenceError(StudioHdr studioHdr, MStudioSeqDesc seqdesc, int sequence, TimeUnit_t cycle, int rule, ReadOnlySpan poseParameter, Span panim, Span weight, ref IKContextIKRule ikRule) { + int i; + + ikRule = default; + ikRule.Start = ikRule.Peak = ikRule.Tail = ikRule.End = 0; + + MStudioIKRule? prevRule = null; + + for (i = 0; i < 4; i++) { + if (weight[i] != 0) { + if (rule >= panim[i].NumIKRules || panim[i].NumIKRules != panim[0].NumIKRules) { + Assert(false); + return false; + } + + MStudioIKRule? pRule = panim[i].IKRule(rule); + if (pRule == null) + return false; + + float dt = 0.0f; + if (prevRule != null) { + if (pRule.Start - prevRule.Start > 0.5) + dt = -1.0f; + else if (pRule.Start - prevRule.Start < -0.5) + dt = 1.0f; + } + else + prevRule = pRule; + + ikRule.Start += (pRule.Start + dt) * weight[i]; + ikRule.Peak += (pRule.Peak + dt) * weight[i]; + ikRule.Tail += (pRule.Tail + dt) * weight[i]; + ikRule.End += (pRule.End + dt) * weight[i]; + } + } + if (ikRule.Start > 1.0) { + ikRule.Start -= 1.0f; + ikRule.Peak -= 1.0f; + ikRule.Tail -= 1.0f; + ikRule.End -= 1.0f; + } + else if (ikRule.Start < 0.0) { + ikRule.Start += 1.0f; + ikRule.Peak += 1.0f; + ikRule.Tail += 1.0f; + ikRule.End += 1.0f; + } + + ikRule.Weight = Studio_IKRuleWeight(ref ikRule, cycle); + if (ikRule.Weight <= 0.001f) { + if (panim[0].IKRule(rule) == null) + return false; + if ((panim[0].Flags & StudioAnimSeqFlags.Looping) != 0 && panim[0].IKRule(rule)!.Type == Studio.IK_GROUND && ikRule.End - ikRule.Start > 0.75) { + ikRule.Weight = 0.001f; + cycle = ikRule.End - 0.001; + } + else + return false; + } + + Assert(ikRule.Weight > 0.0f); + + ikRule.Pos = default; + ikRule.Q = default; + + float total = 0.0f; + for (i = 0; i < 4; i++) { + if (weight[i] != 0) { + MStudioIKRule? pRule = panim[i].IKRule(rule); + if (pRule == null) + return false; + + ikRule.Chain = pRule.Chain; + ikRule.Bone = pRule.Bone; + ikRule.Type = pRule.Type; + ikRule.Slot = pRule.Slot; + + ikRule.Height += pRule.Height * weight[i]; + ikRule.Floor += pRule.Floor * weight[i]; + ikRule.Radius += pRule.Radius * weight[i]; + ikRule.Drop += pRule.Drop * weight[i]; + ikRule.Top += pRule.Top * weight[i]; + + ikRule.Release += Studio_IKTail(ref ikRule, cycle) * weight[i]; + + switch (ikRule.Type) { + case Studio.IK_SELF: + case Studio.IK_WORLD: + case Studio.IK_GROUND: + case Studio.IK_ATTACHMENT: { + bool result = Studio_IKAnimationError(studioHdr, pRule, panim[i], cycle, out Vector3 pos1, out Quaternion q1, out _); + + if (result) { + ikRule.Pos = ikRule.Pos + pos1 * weight[i]; + QuaternionAccumulate(ikRule.Q, weight[i], q1, out ikRule.Q); + total += weight[i]; + } + } + break; + default: + total += weight[i]; + break; + } + + ikRule.Latched = (Studio_IKShouldLatch(ref ikRule, cycle) ? 1 : 0) * ikRule.Weight; + + if (ikRule.Type == Studio.IK_ATTACHMENT) + ikRule.Label = pRule.Attachment(); + } + } + + if (total <= 0.0001f) + return false; + + if (total < 0.999f) { + ikRule.Pos = ikRule.Pos * (1.0f / total); + MathLib.QuaternionScale(ikRule.Q, 1.0f / total, out Quaternion scaledQ); + ikRule.Q = scaledQ; + } + + if (ikRule.Type == Studio.IK_SELF && ikRule.Bone != -1) { + ikRule.Bone = studioHdr.RemapSeqBone(sequence, ikRule.Bone); + if (ikRule.Bone == -1) + return false; + } + + MathLib.QuaternionNormalize(ref ikRule.Q); + return true; + } +} diff --git a/Source.Common/Mathematics/MathLib.cs b/Source.Common/Mathematics/MathLib.cs index 9c6a1bd0f..e7e1c9ea1 100644 --- a/Source.Common/Mathematics/MathLib.cs +++ b/Source.Common/Mathematics/MathLib.cs @@ -1132,6 +1132,22 @@ public static void QuaternionMult(in Quaternion p, in Quaternion q, out Quaterni qt.Z = p.X * q2.Y - p.Y * q2.X + p.Z * q2.W + p.W * q2.Z; qt.W = -p.X * q2.X - p.Y * q2.Y - p.Z * q2.Z + p.W * q2.W; } + public static float QuaternionNormalize(ref Quaternion q) { + float radius, iradius; + + radius = q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W; + + if (radius != 0) { + radius = MathF.Sqrt(radius); + iradius = 1.0f / radius; + q.W *= iradius; + q.Z *= iradius; + q.Y *= iradius; + q.X *= iradius; + } + return radius; + } + public static void QuaternionScale(in Quaternion p, float t, out Quaternion q) { float r; float sinom = MathF.Sqrt(DotProduct(in p, in p)); diff --git a/Source.Common/Studio.cs b/Source.Common/Studio.cs index a46b7fb4d..ce047d563 100644 --- a/Source.Common/Studio.cs +++ b/Source.Common/Studio.cs @@ -2107,6 +2107,14 @@ public bool SequencesAvailable() { public int GetNumSeq() => vModel == null ? studioHdr!.NumLocalSeq : vModel.Seq.Count; + public int RemapSeqBone(int sequence, int localBone) { + if (vModel != null) { + VirtualGroup seqGroup = vModel.SeqGroup(sequence)!; + return seqGroup.MasterBone[localBone]; + } + return localBone; + } + public StudioHeader? GroupStudioHdr(int i) { if (FrameUnlockCounter != FrameUnlockCounterPtr.Get()) { lock (FrameUnlockCounterMutex) { From 43a9ff7ed29bc6b3da5e3a13416275a7657247e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 19:09:38 +0300 Subject: [PATCH 05/14] Port IK target --- Source.Common/BoneSetupIK.cs | 162 +++++++++++++++++++++++++++++++++++ 1 file changed, 162 insertions(+) diff --git a/Source.Common/BoneSetupIK.cs b/Source.Common/BoneSetupIK.cs index 7a60fcded..69ee6b6b5 100644 --- a/Source.Common/BoneSetupIK.cs +++ b/Source.Common/BoneSetupIK.cs @@ -480,3 +480,165 @@ public static bool Studio_IKSequenceError(StudioHdr studioHdr, MStudioSeqDesc se return true; } } + +public class IKTarget +{ + public struct OffsetData + { + public string? AttachmentName; + public Vector3 Pos; + public Quaternion Q; + } + + public struct IdealData + { + public Vector3 Pos; + public Quaternion Q; + } + + public struct EstimateData + { + public float Latched; + public float Release; + public float Height; + public float Floor; + public float Radius; + public TimeUnit_t Time; + public float Weight; + public Vector3 Pos; + public Quaternion Q; + public bool OnWorld; + } + + public struct TraceData + { + public float HipToFoot; + public float HipToKnee; + public float KneeToFoot; + public Vector3 Hip; + public Vector3 Closest; + public Vector3 Knee; + public Vector3 Farthest; + public Vector3 Lowest; + } + + public struct LatchedData + { + public bool NeedsLatch; + public bool HasLatch; + public float Influence; + public int FrameCounter; + public int Owner; + public Vector3 AbsOrigin; + public QAngle AbsAngles; + public Vector3 Pos; + public Quaternion Q; + public Vector3 DeltaPos; + public Quaternion DeltaQ; + public Vector3 DebouncePos; + public Quaternion DebounceQ; + } + + public struct ErrorData + { + public TimeUnit_t Time; + public TimeUnit_t ErrorTime; + public float Ramp; + public bool InError; + } + + public int Chain; + public int Type; + + public OffsetData Offset; + internal IdealData Ideal; + public EstimateData Est; + public TraceData Trace; + internal LatchedData Latched; + public ErrorData Error; + + public void SetOwner(int entindex, in Vector3 pos, in QAngle angles) { + Latched.Owner = entindex; + Latched.AbsOrigin = pos; + Latched.AbsAngles = angles; + } + + public void ClearOwner() { + Latched.Owner = -1; + } + + public int GetOwner() => Latched.Owner; + + public void UpdateOwner(int entindex, in Vector3 pos, in QAngle angles) { + if (pos == Latched.AbsOrigin && angles == Latched.AbsAngles) + return; + + MathLib.AngleMatrix(angles, pos, out Matrix3x4 inMat); + MathLib.AngleIMatrix(Latched.AbsAngles, Latched.AbsOrigin, out Matrix3x4 outMat); + + MathLib.QuaternionMatrix(Latched.Q, Latched.Pos, out Matrix3x4 tmp1); + MathLib.ConcatTransforms(outMat, tmp1, out Matrix3x4 tmp2); + MathLib.ConcatTransforms(inMat, tmp2, out tmp1); + MathLib.MatrixAngles(tmp1, out Latched.Q, out Latched.Pos); + } + + public void SetPos(in Vector3 pos) { + Est.Pos = pos; + } + + public void SetAngles(in QAngle angles) { + MathLib.AngleQuaternion(angles, out Est.Q); + } + + public void SetQuaternion(in Quaternion q) { + Est.Q = q; + } + + public void SetNormal(in Vector3 normal) { + Vector3 forward, right; + MathLib.QuaternionMatrix(Est.Q, out Matrix3x4 m1); + + MathLib.MatrixGetColumn(m1, 1, out right); + MathLib.CrossProduct(right, normal, out forward); + MathLib.CrossProduct(normal, forward, out right); + MathLib.MatrixSetColumn(forward, 0, ref m1); + MathLib.MatrixSetColumn(right, 1, ref m1); + MathLib.MatrixSetColumn(normal, 2, ref m1); + MathLib.MatrixAngles(m1, out Est.Q, out _); + } + + public void SetPosWithNormalOffset(in Vector3 pos, in Vector3 normal) { + Est.Pos = pos; + if (normal.Z > 0.9999) + return; + else if (normal.Z > 0.707) { + float tan = MathF.Sqrt(1 - normal.Z * normal.Z) / normal.Z; + Est.Pos.Z = Est.Pos.Z - Est.Radius * tan; + } + else + Est.Pos.Z = Est.Pos.Z - Est.Radius; + } + + public void SetOnWorld(bool onWorld = true) { + Est.OnWorld = onWorld; + } + + public bool IsActive() => Est.Weight > 0.0f; + + public void IKFailed() { + Latched.DeltaPos = default; + Latched.DeltaQ = default; + Latched.Pos = Ideal.Pos; + Latched.Q = Ideal.Q; + Est.Latched = 0.0f; + Est.Weight = 0.0f; + Est.OnWorld = false; + } + + public void MoveReferenceFrame(in Vector3 deltaPos, in QAngle deltaAngles) { + Est.Pos -= deltaPos; + Latched.Pos -= deltaPos; + Offset.Pos -= deltaPos; + Ideal.Pos -= deltaPos; + } +} From 93e34f9500c95c3b5c4ddfbc524b1bfdd58d729d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 20:19:05 +0300 Subject: [PATCH 06/14] Port IK context --- Source.Common/BoneSetup.cs | 6 +- Source.Common/BoneSetupIK.cs | 766 +++++++++++++++++++++++++++++++++++ 2 files changed, 769 insertions(+), 3 deletions(-) diff --git a/Source.Common/BoneSetup.cs b/Source.Common/BoneSetup.cs index 6a1676af7..c2b261b72 100644 --- a/Source.Common/BoneSetup.cs +++ b/Source.Common/BoneSetup.cs @@ -375,7 +375,7 @@ private static bool CalcPoseSingle(StudioHdr studioHdr, Span pos, Span< return bResult; } - private static void Studio_SeqAnims(StudioHdr studioHdr, MStudioSeqDesc seqdesc, int sequence, ReadOnlySpan poseParameter, Span panim, Span weight) { + internal static void Studio_SeqAnims(StudioHdr studioHdr, MStudioSeqDesc seqdesc, int sequence, ReadOnlySpan poseParameter, Span panim, Span weight) { if (studioHdr == null || sequence >= studioHdr.GetNumSeq()) { weight[0] = weight[1] = weight[2] = weight[3] = 0.0f; return; @@ -402,7 +402,7 @@ private static void Studio_SeqAnims(StudioHdr studioHdr, MStudioSeqDesc seqdesc, Assert(weight[0] >= 0.0f && weight[1] >= 0.0f && weight[2] >= 0.0f && weight[3] >= 0.0f); } - private static float Studio_CPS(StudioHdr studioHdr, MStudioSeqDesc seqdesc, int sequence, ReadOnlySpan poseParameter) { + internal static float Studio_CPS(StudioHdr studioHdr, MStudioSeqDesc seqdesc, int sequence, ReadOnlySpan poseParameter) { MStudioAnimDesc[] panim = ArrayPool.Shared.Rent(4); Span weight = stackalloc float[4]; @@ -1344,7 +1344,7 @@ private static void WorldSpaceSlerp(StudioHdr studioHdr, Span q1, Sp MatrixPool.Free(targetBoneToWorld); } - private static void BuildBoneChain(StudioHdr studioHdr, in Matrix3x4 rootxform, ReadOnlySpan pos, ReadOnlySpan q, int bone, Matrix3x4[] boneToWorld, ref BoneBitList boneComputed) { + internal static void BuildBoneChain(StudioHdr studioHdr, in Matrix3x4 rootxform, ReadOnlySpan pos, ReadOnlySpan q, int bone, Span boneToWorld, ref BoneBitList boneComputed) { if (boneComputed.IsBoneMarked(bone)) return; diff --git a/Source.Common/BoneSetupIK.cs b/Source.Common/BoneSetupIK.cs index 69ee6b6b5..2d378d996 100644 --- a/Source.Common/BoneSetupIK.cs +++ b/Source.Common/BoneSetupIK.cs @@ -1,6 +1,8 @@ using Source.Common.Mathematics; +using System.Buffers; using System.Numerics; +using System.Runtime.InteropServices; namespace Source.Common; @@ -353,6 +355,11 @@ public static bool Studio_IKAnimationError(StudioHdr studioHdr, MStudioIKRule ru public static bool Studio_IKSequenceError(StudioHdr studioHdr, MStudioSeqDesc seqdesc, int sequence, TimeUnit_t cycle, int rule, ReadOnlySpan poseParameter, Span panim, Span weight, ref IKContextIKRule ikRule) { int i; + if (panim[0] == null) { + Warning("Crash prevented: Studio_IKSequenceError: panim has a nullptr element\n"); + return false; + } + ikRule = default; ikRule.Start = ikRule.Peak = ikRule.Tail = ikRule.End = 0; @@ -642,3 +649,762 @@ public void MoveReferenceFrame(in Vector3 deltaPos, in QAngle deltaAngles) { Ideal.Pos -= deltaPos; } } + +public struct IKChainResult +{ + public int Target; + public Vector3 Pos; + public Quaternion Q; + public float Weight; +} + +public ref partial struct BoneSetup +{ + public static void SolveBone(StudioHdr studioHdr, int bone, Span boneToWorld, Span pos, Span q) { + int parent = studioHdr.BoneParent(bone); + + MathLib.MatrixInvert(boneToWorld[parent], out Matrix3x4 worldToBone); + + MathLib.ConcatTransforms(worldToBone, boneToWorld[bone], out Matrix3x4 local); + + MathLib.MatrixAngles(local, out q[bone], out pos[bone]); + } +} + +public class IKContext +{ + static readonly BoneSetupMemoryPool MatrixPool = new(); + + public readonly List Target = new(12); + + StudioHdr? studioHdr; + + readonly List> ikChainRule = []; + readonly List ikLock = []; + Matrix3x4 rootxform; + + int frameCounter; + TimeUnit_t time; + int boneMask; + + public IKContext() { + frameCounter = -1; + studioHdr = null; + time = -1.0; + } + + void ResetTargets() { + Target.Clear(); + for (int i = 0; i < 12; i++) + Target.Add(new IKTarget()); + ClearTargets(); + } + + public void Init(StudioHdr studioHdr, in QAngle angles, in Vector3 pos, TimeUnit_t time, int frameCounter, int boneMask) { + this.studioHdr = studioHdr; + ikChainRule.Clear(); + if (studioHdr.GetNumIKChains() != 0) { + for (int i = 0; i < studioHdr.GetNumIKChains(); i++) + ikChainRule.Add([]); + + if (Target.Count == 0) + ResetTargets(); + } + else + Target.Clear(); + + MathLib.AngleMatrix(angles, pos, out rootxform); + this.frameCounter = frameCounter; + this.time = time; + this.boneMask = boneMask; + } + + public void AddDependencies(MStudioSeqDesc seqdesc, int sequence, TimeUnit_t cycle, ReadOnlySpan poseParameters, float weight = 1.0f) { + int i; + + if (studioHdr!.GetNumIKChains() == 0) + return; + + if (seqdesc.NumIKRules == 0) + return; + + IKContextIKRule ikrule = default; + + Assert(weight >= 0.0f && weight <= 1.0f); + weight = Math.Clamp(weight, 0.0f, 1.0f); + + if ((seqdesc.Flags & StudioAnimSeqFlags.Realtime) != 0) { + float cps = BoneSetup.Studio_CPS(studioHdr, seqdesc, sequence, poseParameters); + cycle = time * cps; + cycle = cycle - (int)cycle; + } + else if (cycle < 0 || cycle >= 1) { + if ((seqdesc.Flags & StudioAnimSeqFlags.Looping) != 0) { + cycle = cycle - (int)cycle; + if (cycle < 0) + cycle += 1; + } + else + cycle = Math.Max(0.0, Math.Min(cycle, 0.9999)); + } + + MStudioAnimDesc[] panim = ArrayPool.Shared.Rent(4); + Span animWeight = stackalloc float[4]; + + BoneSetup.Studio_SeqAnims(studioHdr, seqdesc, sequence, poseParameters, panim, animWeight); + + for (i = 0; i < seqdesc.NumIKRules; i++) { + if (!BoneSetup.Studio_IKSequenceError(studioHdr, seqdesc, sequence, cycle, i, poseParameters, panim, animWeight, ref ikrule)) + continue; + + if (ikrule.Chain < 0 || ikrule.Chain >= studioHdr.GetNumIKChains() || studioHdr.IKChain(ikrule.Chain).NumLinks <= 2) + continue; + + int bone = studioHdr.IKChain(ikrule.Chain).Link(2).Bone; + if (bone >= studioHdr.NumBones() || ikrule.Bone >= studioHdr.NumBones()) { + Warning($"Out of bounds bone in CIKContext::AddDependencies {bone} / {ikrule.Bone} of {studioHdr.NumBones()}\n"); + continue; + } + + if ((studioHdr.BoneFlags(bone) & boneMask) == 0) + continue; + + if (ikrule.Bone >= 0 && (studioHdr.BoneFlags(ikrule.Bone) & boneMask) == 0) + continue; + + if (Target.Count == 0) + ResetTargets(); + + ikrule.RuleWeight = weight; + + if (ikrule.RuleWeight * ikrule.Weight > 0.999) { + if (ikrule.Type != Studio.IK_UNLATCH) { + ikChainRule[ikrule.Chain].Clear(); + if (ikrule.Type == Studio.IK_RELEASE) + continue; + } + } + + ikChainRule[ikrule.Chain].Add(ikrule); + } + + ArrayPool.Shared.Return(panim, true); + } + + public void AddAutoplayLocks(Span pos, Span q) { + if (studioHdr!.GetNumIKAutoplayLocks() == 0) + return; + + if (studioHdr.GetNumIKChains() == 0 && studioHdr.GetNumIKAutoplayLocks() != 0) + return; + + Matrix3x4[] boneToWorld = MatrixPool.Alloc(); + BoneBitList boneComputed = new(); + + int ikOffset = ikLock.Count; + for (int i = 0; i < studioHdr.GetNumIKAutoplayLocks(); i++) + ikLock.Add(default); + + for (int i = 0; i < studioHdr.GetNumIKAutoplayLocks(); i++) { + MStudioIKLock lck = studioHdr.IKAutoplayLock(i); + + if (!IsValidChain(lck.Chain)) { + Warning($"Model has IK errors.. {lck.Chain}/{studioHdr.GetNumIKChains()}\n"); + MatrixPool.Free(boneToWorld); + return; + } + + MStudioIKChain chain = studioHdr.IKChain(lck.Chain); + int bone = chain.Link(2).Bone; + + if ((studioHdr.BoneFlags(bone) & boneMask) == 0) + continue; + + BuildBoneChain(pos, q, bone, boneToWorld, ref boneComputed); + + ref IKContextIKRule ikrule = ref CollectionsMarshal.AsSpan(ikLock)[i + ikOffset]; + + ikrule.Chain = lck.Chain; + ikrule.Slot = i; + ikrule.Type = Studio.IK_WORLD; + + MathLib.MatrixAngles(boneToWorld[bone], out ikrule.Q, out ikrule.Pos); + + if (chain.Link(0).KneeDir.LengthSquared() > 0.0) { + MathLib.VectorRotate(chain.Link(0).KneeDir, boneToWorld[chain.Link(0).Bone], out ikrule.KneeDir); + MathLib.MatrixPosition(boneToWorld[chain.Link(1).Bone], out ikrule.KneePos); + } + else + ikrule.KneeDir = default; + } + MatrixPool.Free(boneToWorld); + } + + public void AddSequenceLocks(MStudioSeqDesc seqdesc, Span pos, Span q) { + if (studioHdr!.GetNumIKChains() == 0) + return; + + if (seqdesc.NumIKLocks == 0) + return; + + Matrix3x4[] boneToWorld = MatrixPool.Alloc(); + BoneBitList boneComputed = new(); + + int ikOffset = ikLock.Count; + for (int i = 0; i < seqdesc.NumIKLocks; i++) + ikLock.Add(default); + + for (int i = 0; i < seqdesc.NumIKLocks; i++) { + MStudioIKLock lck = seqdesc.IKLock(i); + if (!IsValidChain(lck.Chain)) + continue; + + MStudioIKChain chain = studioHdr.IKChain(lck.Chain); + int bone = chain.Link(2).Bone; + + if ((studioHdr.BoneFlags(bone) & boneMask) == 0) + continue; + + BuildBoneChain(pos, q, bone, boneToWorld, ref boneComputed); + + ref IKContextIKRule ikrule = ref CollectionsMarshal.AsSpan(ikLock)[i + ikOffset]; + ikrule.Chain = i; + ikrule.Slot = i; + ikrule.Type = Studio.IK_WORLD; + + MathLib.MatrixAngles(boneToWorld[bone], out ikrule.Q, out ikrule.Pos); + + if (chain.Link(0).KneeDir.LengthSquared() > 0.0) + MathLib.VectorRotate(chain.Link(0).KneeDir, boneToWorld[chain.Link(0).Bone], out ikrule.KneeDir); + else + ikrule.KneeDir = default; + } + MatrixPool.Free(boneToWorld); + } + + bool IsValidChain(int chain) { + if (chain < 0 || chain >= studioHdr!.GetNumIKChains()) + return false; + + MStudioIKChain ikChain = studioHdr.IKChain(chain); + if (ikChain.NumLinks < 3) + return false; + + for (int i = 0; i < 3; i++) { + int bone = ikChain.Link(i).Bone; + if (bone < 0 || bone >= studioHdr.NumBones()) + return false; + } + + return true; + } + + void BuildBoneChain(ReadOnlySpan pos, ReadOnlySpan q, int bone, Span boneToWorld, ref BoneBitList boneComputed) { + Assert((studioHdr!.BoneFlags(bone) & boneMask) != 0); + BoneSetup.BuildBoneChain(studioHdr, rootxform, pos, q, bone, boneToWorld, ref boneComputed); + } + + public void ClearTargets() { + for (int i = 0; i < Target.Count; i++) + Target[i].Latched.FrameCounter = -9999; + } + + public void UpdateTargets(Span pos, Span q, Span boneToWorld, ref BoneBitList boneComputed) { + int i, j; + + for (i = 0; i < Target.Count; i++) { + Target[i].Est.Weight = 0.0f; + Target[i].Est.Latched = 1.0f; + Target[i].Est.Release = 1.0f; + Target[i].Est.Height = 0.0f; + Target[i].Est.Floor = 0.0f; + Target[i].Est.Radius = 0.0f; + Target[i].Offset.Pos = default; + Target[i].Offset.Q = default; + } + + AutoIKRelease(); + + for (j = 0; j < ikChainRule.Count; j++) { + for (i = 0; i < ikChainRule[j].Count; i++) { + ref IKContextIKRule rule = ref CollectionsMarshal.AsSpan(ikChainRule[j])[i]; + + if (rule.Slot < 0 || rule.Slot >= Target.Count) + continue; + + switch (rule.Type) { + case Studio.IK_ATTACHMENT: + case Studio.IK_GROUND: { + IKTarget target = Target[rule.Slot]; + target.Chain = rule.Chain; + target.Type = rule.Type; + + if (rule.Type == Studio.IK_ATTACHMENT) + target.Offset.AttachmentName = rule.Label; + else + target.Offset.AttachmentName = null; + + if (rule.RuleWeight == 1.0f || target.Est.Weight == 0.0f) { + target.Offset.Q = rule.Q; + target.Offset.Pos = rule.Pos; + target.Est.Height = rule.Height; + target.Est.Floor = rule.Floor; + target.Est.Radius = rule.Radius; + target.Est.Latched = rule.Latched * rule.RuleWeight; + target.Est.Release = rule.Release; + target.Est.Weight = rule.Weight * rule.RuleWeight; + } + else { + MathLib.QuaternionSlerp(target.Offset.Q, rule.Q, rule.RuleWeight, out Quaternion offsetQ); + target.Offset.Q = offsetQ; + target.Offset.Pos = target.Offset.Pos + (rule.Pos - target.Offset.Pos) * rule.RuleWeight; + target.Est.Height = MathLib.Lerp(rule.RuleWeight, target.Est.Height, rule.Height); + target.Est.Floor = MathLib.Lerp(rule.RuleWeight, target.Est.Floor, rule.Floor); + target.Est.Radius = MathLib.Lerp(rule.RuleWeight, target.Est.Radius, rule.Radius); + target.Est.Latched = Math.Min(target.Est.Latched, rule.Latched); + target.Est.Release = MathLib.Lerp(rule.RuleWeight, target.Est.Release, rule.Release); + target.Est.Weight = MathLib.Lerp(rule.RuleWeight, target.Est.Weight, rule.Weight); + } + + if (rule.Type == Studio.IK_GROUND) { + target.Latched.DeltaPos.Z = 0; + target.Est.Pos.Z = target.Est.Floor + rootxform[2][3]; + } + } + break; + case Studio.IK_UNLATCH: { + IKTarget target = Target[rule.Slot]; + if (rule.Latched > 0.0) + target.Est.Latched = 0.0f; + else + target.Est.Latched = Math.Min(target.Est.Latched, 1.0f - rule.Weight); + } + break; + case Studio.IK_RELEASE: { + IKTarget target = Target[rule.Slot]; + if (rule.Latched > 0.0) + target.Est.Latched = 0.0f; + else + target.Est.Latched = Math.Min(target.Est.Latched, 1.0f - rule.Weight); + + target.Est.Weight = target.Est.Weight * (1 - rule.Weight * rule.RuleWeight); + } + break; + } + } + } + + for (i = 0; i < Target.Count; i++) { + IKTarget target = Target[i]; + if (target.Est.Weight > 0.0) { + MStudioIKChain chain = studioHdr!.IKChain(target.Chain); + int bone = chain.Link(2).Bone; + + BuildBoneChain(pos, q, bone, boneToWorld, ref boneComputed); + + MathLib.QuaternionMatrix(target.Offset.Q, target.Offset.Pos, out Matrix3x4 local); + MathLib.MatrixInvert(local, out Matrix3x4 localInv); + MathLib.ConcatTransforms(boneToWorld[bone], localInv, out Matrix3x4 worldFootpad); + + if (target.Est.Latched == 1.0) + target.Latched.NeedsLatch = true; + else + target.Latched.NeedsLatch = false; + + if (frameCounter < 0 || target.Latched.FrameCounter < frameCounter - 1 || target.Latched.FrameCounter > frameCounter) { + target.Latched.HasLatch = false; + target.Latched.Influence = 0.0f; + } + target.Latched.FrameCounter = frameCounter; + + MathLib.MatrixAngles(worldFootpad, out target.Ideal.Q, out target.Ideal.Pos); + target.Est.Q = target.Ideal.Q; + target.Est.Pos = target.Ideal.Pos; + + float latched = target.Est.Latched; + + if (target.Latched.HasLatch) { + if (target.Est.Latched == 1.0) { + target.Latched.DeltaPos = target.Latched.Pos - target.Est.Pos; + MathLib.QuaternionSM(-1, target.Est.Q, target.Latched.Q, ref target.Latched.DeltaQ); + target.Est.Q = target.Latched.Q; + target.Est.Pos = target.Latched.Pos; + } + else if (target.Est.Latched > 0.0) { + if (latched > 0 && latched < target.Latched.Influence) { + float dt = target.Latched.Influence - latched; + if (target.Latched.Influence > 0.0) + dt = dt / target.Latched.Influence; + + target.Latched.DeltaPos = target.Latched.DeltaPos * (1 - dt); + MathLib.QuaternionScale(target.Latched.DeltaQ, 1 - dt, out Quaternion deltaQ); + target.Latched.DeltaQ = deltaQ; + } + + target.Est.Pos = target.Est.Pos + target.Latched.DeltaPos; + MathLib.QuaternionMA(target.Est.Q, 1, target.Latched.DeltaQ, ref target.Est.Q); + target.Latched.Q = target.Est.Q; + target.Latched.Pos = target.Est.Pos; + } + else { + target.Latched.HasLatch = false; + target.Latched.Q = target.Est.Q; + target.Latched.Pos = target.Est.Pos; + target.Latched.DeltaPos = default; + target.Latched.DeltaQ = default; + } + target.Latched.Influence = latched; + } + + MathLib.MatrixPosition(boneToWorld[chain.Link(0).Bone], out Vector3 p1); + MathLib.MatrixPosition(boneToWorld[chain.Link(1).Bone], out Vector3 p2); + MathLib.MatrixPosition(boneToWorld[chain.Link(2).Bone], out Vector3 p3); + + float d1 = (p2 - p1).Length(); + float d2 = (p3 - p2).Length(); + + if (target.Latched.HasLatch) { + float d4 = (p3 + target.Latched.DeltaPos - p1).Length(); + + if ((d4 < MathF.Abs(d1 - d2) || d4 * 0.95 > d1 + d2) && target.Est.Latched > 0.2) + target.Error.Time = time; + + if (target.Est.Latched > 0.2) { + float d = MathF.Abs(target.Latched.DeltaQ.W) * 2.0f - 1.0f; + + if (d < 0.707) + target.Error.Time = time; + } + } + + Vector3 dir = target.Est.Pos - p1; + target.Trace.HipToFoot = MathLib.VectorNormalize(ref dir); + target.Trace.HipToKnee = d1; + target.Trace.KneeToFoot = d2; + target.Trace.Hip = p1; + target.Trace.Knee = p2; + target.Trace.Closest = p1 + dir * (float)(MathF.Abs(d1 - d2) * 1.01); + target.Trace.Farthest = p1 + dir * (d1 + d2) * 0.99f; + target.Trace.Lowest = p1 + new Vector3(0, 0, -1) * (d1 + d2) * 0.99f; + } + } + } + + public void AutoIKRelease() { + int i; + + for (i = 0; i < Target.Count; i++) { + IKTarget target = Target[i]; + + TimeUnit_t dt = time - target.Error.Time; + if (target.Error.InError || dt < 0.5) { + if (!target.Error.InError) { + target.Error.Ramp = 0.0f; + target.Error.ErrorTime = target.Error.Time; + target.Error.InError = true; + } + + TimeUnit_t ft = time - target.Error.ErrorTime; + if (dt < 0.25) + target.Error.Ramp = (float)Math.Min(target.Error.Ramp + ft * 4.0, 1.0); + else + target.Error.Ramp = (float)Math.Max(target.Error.Ramp - ft * 4.0, 0.0); + + if (target.Error.Ramp > 0.0) { + IKContextIKRule ikrule = default; + + ikrule.Chain = target.Chain; + ikrule.Bone = 0; + ikrule.Type = Studio.IK_RELEASE; + ikrule.Slot = i; + ikrule.Weight = MathLib.SimpleSpline(target.Error.Ramp); + ikrule.RuleWeight = 1.0f; + ikrule.Latched = dt < 0.25 ? 0.0f : ikrule.Weight; + + if (IsValidChain(target.Chain)) { + MStudioIKChain chain = studioHdr.IKChain(target.Chain); + int bone = chain.Link(2).Bone; + if (bone >= 0 && bone < studioHdr.NumBones()) { + if ((studioHdr.BoneFlags(bone) & boneMask) == 0) { + target.Error.InError = false; + continue; + } + + ikChainRule[ikrule.Chain].Add(ikrule); + } + else + DevWarning(1, $"AutoIKRelease ({studioHdr.GetRenderHdr().GetName()}) got an out of range bone {bone} ({studioHdr.NumBones()})\n"); + } + else + DevWarning(1, $"AutoIKRelease ({studioHdr!.GetRenderHdr().GetName()}) got an out of range chain {target.Chain} ({studioHdr.GetNumIKChains()})\n"); + } + else + target.Error.InError = false; + + target.Error.ErrorTime = time; + } + } + } + + public void SolveDependencies(Span pos, Span q, Span boneToWorld, ref BoneBitList boneComputed) { + Matrix3x4 worldTarget; + int i, j; + + Span chainResult = stackalloc IKChainResult[32]; + chainResult.Clear(); + + for (i = 0; i < studioHdr!.GetNumIKChains(); i++) { + if (i >= 32) + continue; + + ref IKChainResult chainRes = ref chainResult[i]; + + chainRes.Target = -1; + chainRes.Weight = 0.0f; + + if (!IsValidChain(i)) + continue; + + MStudioIKChain chain = studioHdr.IKChain(i); + int bone = chain.Link(2).Bone; + + if ((studioHdr.BoneFlags(bone) & boneMask) == 0) + continue; + + BuildBoneChain(pos, q, bone, boneToWorld, ref boneComputed); + + MathLib.MatrixAngles(boneToWorld[bone], out chainRes.Q, out chainRes.Pos); + } + + for (j = 0; j < ikChainRule.Count; j++) { + for (i = 0; i < ikChainRule[j].Count; i++) { + ref IKContextIKRule rule = ref CollectionsMarshal.AsSpan(ikChainRule[j])[i]; + if ((uint)rule.Chain >= 32) + continue; + + ref IKChainResult chainRes = ref chainResult[rule.Chain]; + chainRes.Target = -1; + + switch (rule.Type) { + case Studio.IK_SELF: { + MathLib.QuaternionMatrix(rule.Q, rule.Pos, out Matrix3x4 local); + if (rule.Bone != -1) { + BuildBoneChain(pos, q, rule.Bone, boneToWorld, ref boneComputed); + MathLib.ConcatTransforms(boneToWorld[rule.Bone], local, out worldTarget); + } + else + MathLib.ConcatTransforms(rootxform, local, out worldTarget); + + float weight = rule.Weight * rule.RuleWeight; + chainRes.Weight = chainRes.Weight * (1 - weight) + weight; + + MathLib.MatrixAngles(worldTarget, out Quaternion q2, out Vector3 p2); + + chainRes.Pos = chainRes.Pos * (float)(1.0 - weight) + p2 * weight; + MathLib.QuaternionSlerp(chainRes.Q, q2, weight, out Quaternion blendedQ); + chainRes.Q = blendedQ; + } + break; + case Studio.IK_WORLD: + Assert(false); + break; + + case Studio.IK_ATTACHMENT: + break; + + case Studio.IK_GROUND: + break; + + case Studio.IK_RELEASE: { + float weight = rule.Weight * rule.RuleWeight; + MStudioIKChain chain = studioHdr.IKChain(rule.Chain); + int bone = chain.Link(2).Bone; + + BuildBoneChain(pos, q, bone, boneToWorld, ref boneComputed); + MathLib.MatrixAngles(boneToWorld[bone], out Quaternion q2, out Vector3 p2); + + chainRes.Pos = chainRes.Pos * (float)(1.0 - weight) + p2 * weight; + MathLib.QuaternionSlerp(chainRes.Q, q2, weight, out Quaternion blendedQ); + chainRes.Q = blendedQ; + } + break; + case Studio.IK_UNLATCH: + break; + } + } + } + + for (i = 0; i < Target.Count; i++) { + IKTarget target = Target[i]; + + if ((uint)target.Chain >= 32) + continue; + + if (Target[i].Est.Weight > 0.0) { + ref IKChainResult chainRes = ref chainResult[target.Chain]; + + MathLib.QuaternionAngles(target.Offset.Q, out RadianEuler offsetAngles); + MathLib.AngleMatrix(offsetAngles, target.Offset.Pos, out Matrix3x4 local); + + MathLib.QuaternionAngles(target.Est.Q, out RadianEuler estAngles); + MathLib.AngleMatrix(estAngles, target.Est.Pos, out Matrix3x4 worldFootpad); + + MathLib.ConcatTransforms(worldFootpad, local, out worldTarget); + + MathLib.MatrixAngles(worldTarget, out Quaternion q2, out Vector3 p2); + + chainRes.Weight = target.Est.Weight; + chainRes.Pos = chainRes.Pos * (float)(1.0 - chainRes.Weight) + p2 * chainRes.Weight; + MathLib.QuaternionSlerp(chainRes.Q, q2, chainRes.Weight, out Quaternion blendedQ); + chainRes.Q = blendedQ; + } + + if (target.Latched.NeedsLatch) { + target.Latched.HasLatch = true; + target.Latched.Q = target.Est.Q; + target.Latched.Pos = target.Est.Pos; + } + } + + for (i = 0; i < studioHdr.GetNumIKChains(); i++) { + if (i >= 32) + continue; + + ref IKChainResult chainRes = ref chainResult[i]; + MStudioIKChain chain = studioHdr.IKChain(i); + + if (chainRes.Weight > 0.0) { + if (BoneSetup.Studio_SolveIK(chain, chainRes.Pos, boneToWorld)) { + MathLib.MatrixGetColumn(boneToWorld[chain.Link(2).Bone], 3, out Vector3 p3); + MathLib.QuaternionMatrix(chainRes.Q, p3, out boneToWorld[chain.Link(2).Bone]); + + BoneSetup.SolveBone(studioHdr, chain.Link(2).Bone, boneToWorld, pos, q); + BoneSetup.SolveBone(studioHdr, chain.Link(1).Bone, boneToWorld, pos, q); + BoneSetup.SolveBone(studioHdr, chain.Link(0).Bone, boneToWorld, pos, q); + } + else { + if (chainRes.Target != -1) { + IKTarget target = Target[chainRes.Target]; + target.Latched.DeltaPos = target.Latched.DeltaPos * 0.8f; + MathLib.QuaternionScale(target.Latched.DeltaQ, 0.8f, out Quaternion deltaQ); + target.Latched.DeltaQ = deltaQ; + } + } + } + } + } + + public void SolveAutoplayLocks(Span pos, Span q) { + Matrix3x4[] boneToWorld = MatrixPool.Alloc(); + BoneBitList boneComputed = new(); + int i; + + for (i = 0; i < ikLock.Count; i++) { + MStudioIKLock lck = studioHdr!.IKAutoplayLock(i); + SolveLock(lck.Chain, lck.PosWeight, lck.LocalQWeight, i, pos, q, boneToWorld, ref boneComputed); + } + MatrixPool.Free(boneToWorld); + } + + public void SolveSequenceLocks(MStudioSeqDesc seqdesc, Span pos, Span q) { + Matrix3x4[] boneToWorld = MatrixPool.Alloc(); + BoneBitList boneComputed = new(); + int i; + + for (i = 0; i < ikLock.Count; i++) { + MStudioIKLock lck = seqdesc.IKLock(i); + SolveLock(lck.Chain, lck.PosWeight, lck.LocalQWeight, i, pos, q, boneToWorld, ref boneComputed); + } + MatrixPool.Free(boneToWorld); + } + + public void AddAllLocks(Span pos, Span q) { + if (studioHdr!.GetNumIKChains() == 0) + return; + + Matrix3x4[] boneToWorld = MatrixPool.Alloc(); + BoneBitList boneComputed = new(); + + int ikOffset = ikLock.Count; + for (int i = 0; i < studioHdr.GetNumIKChains(); i++) + ikLock.Add(default); + + for (int i = 0; i < studioHdr.GetNumIKChains(); i++) { + if (!IsValidChain(i)) + continue; + + MStudioIKChain chain = studioHdr.IKChain(i); + int bone = chain.Link(2).Bone; + + if ((studioHdr.BoneFlags(bone) & boneMask) == 0) + continue; + + BuildBoneChain(pos, q, bone, boneToWorld, ref boneComputed); + + ref IKContextIKRule ikrule = ref CollectionsMarshal.AsSpan(ikLock)[i + ikOffset]; + + ikrule.Chain = i; + ikrule.Slot = i; + ikrule.Type = Studio.IK_WORLD; + + MathLib.MatrixAngles(boneToWorld[bone], out ikrule.Q, out ikrule.Pos); + + if (chain.Link(0).KneeDir.LengthSquared() > 0.0) { + MathLib.VectorRotate(chain.Link(0).KneeDir, boneToWorld[chain.Link(0).Bone], out ikrule.KneeDir); + MathLib.MatrixPosition(boneToWorld[chain.Link(1).Bone], out ikrule.KneePos); + } + else + ikrule.KneeDir = default; + } + MatrixPool.Free(boneToWorld); + } + + public void SolveAllLocks(Span pos, Span q) { + Matrix3x4[] boneToWorld = MatrixPool.Alloc(); + BoneBitList boneComputed = new(); + int i; + + for (i = 0; i < ikLock.Count; i++) + SolveLock(i, 1.0f, 0.0f, i, pos, q, boneToWorld, ref boneComputed); + + MatrixPool.Free(boneToWorld); + } + + public void SolveLock(int lockChain, float lockPosWeight, float lockLocalQWeight, int i, Span pos, Span q, Span boneToWorld, ref BoneBitList boneComputed) { + if (!IsValidChain(lockChain)) + return; + + MStudioIKChain chain = studioHdr!.IKChain(lockChain); + int bone = chain.Link(2).Bone; + + if ((studioHdr.BoneFlags(bone) & boneMask) == 0) + return; + + BuildBoneChain(pos, q, bone, boneToWorld, ref boneComputed); + + Vector3 p1, p3; + Quaternion q2; + + MathLib.MatrixPosition(boneToWorld[bone], out p1); + + ref IKContextIKRule ikLockRule = ref CollectionsMarshal.AsSpan(ikLock)[i]; + + p3 = p1 * (float)(1.0 - lockPosWeight) + ikLockRule.Pos * lockPosWeight; + + if (ikLockRule.KneeDir.LengthSquared() > 0) + BoneSetup.Studio_SolveIK(chain.Link(0).Bone, chain.Link(1).Bone, chain.Link(2).Bone, p3, ikLockRule.KneePos, ikLockRule.KneeDir, boneToWorld); + else + BoneSetup.Studio_SolveIK(chain, p3, boneToWorld); + + MathLib.MatrixPosition(boneToWorld[bone], out p3); + MathLib.QuaternionMatrix(ikLockRule.Q, p3, out boneToWorld[bone]); + + q2 = q[bone]; + BoneSetup.SolveBone(studioHdr, chain.Link(2).Bone, boneToWorld, pos, q); + MathLib.QuaternionSlerp(q[bone], q2, lockLocalQWeight, out Quaternion slerped); + q[bone] = slerped; + + BoneSetup.SolveBone(studioHdr, chain.Link(1).Bone, boneToWorld, pos, q); + BoneSetup.SolveBone(studioHdr, chain.Link(0).Bone, boneToWorld, pos, q); + } +} From c02837e2898e5ba0dc2742186b4637da04874622 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 20:43:51 +0300 Subject: [PATCH 07/14] Hook IK context into pose accumulation --- Source.Common/BoneSetup.cs | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/Source.Common/BoneSetup.cs b/Source.Common/BoneSetup.cs index c2b261b72..d1491285d 100644 --- a/Source.Common/BoneSetup.cs +++ b/Source.Common/BoneSetup.cs @@ -198,7 +198,7 @@ public void InitPose(StudioHdr studioHdr, Span pos, Span q, } } - public unsafe void AccumulatePose(Span pos, Span q, int sequence, TimeUnit_t cycle, float weight, TimeUnit_t time, object? ikContext) { + public unsafe void AccumulatePose(Span pos, Span q, int sequence, TimeUnit_t cycle, float weight, TimeUnit_t time, IKContext? ikContext) { Span pos2 = stackalloc Vector3[Studio.MAXSTUDIOBONES]; Span q2 = stackalloc Quaternion[Studio.MAXSTUDIOBONES]; @@ -211,7 +211,12 @@ public unsafe void AccumulatePose(Span pos, Span q, int seq MStudioSeqDesc seqdesc = studioHdr.Seqdesc(sequence); - // TODO: IK stuff. We're ignoring it for now. + IKContext? seqIK = null; + if (seqdesc.NumIKLocks != 0) { + seqIK = new(); + seqIK.Init(studioHdr, vec3_angle, vec3_origin, 0.0, 0, boneMask); + seqIK.AddSequenceLocks(seqdesc, pos, q); + } if ((seqdesc.Flags & StudioAnimSeqFlags.Local) != 0) InitPose(studioHdr, pos2, q2, boneMask); @@ -221,9 +226,12 @@ public unsafe void AccumulatePose(Span pos, Span q, int seq SlerpBones(studioHdr, q, pos, seqdesc, sequence, q2, pos2, weight, boneMask); } - // TODO: IK + ikContext?.AddDependencies(seqdesc, sequence, cycle, poseParameter, weight); + AddSequenceLayers(pos, q, seqdesc, sequence, cycle, weight, time, ikContext); - // TODO: IK + + if (seqdesc.NumIKLocks != 0) + seqIK!.SolveSequenceLocks(seqdesc, pos, q); } private static bool PoseIsAllZeros(StudioHdr studioHdr, int sequence, MStudioSeqDesc seqdesc, int i0, int i1) { @@ -1143,7 +1151,7 @@ private static void Studio_LocalPoseParameter(StudioHdr studioHdr, ReadOnlySpan< } } - private void AddLocalLayers(Span pos, Span q, MStudioSeqDesc seqdesc, int sequence, double cycle, double weight, double time, object? ikContext) { + private void AddLocalLayers(Span pos, Span q, MStudioSeqDesc seqdesc, int sequence, double cycle, double weight, double time, IKContext? ikContext) { if ((seqdesc.Flags & StudioAnimSeqFlags.Local) == 0) return; @@ -1360,7 +1368,7 @@ internal static void BuildBoneChain(StudioHdr studioHdr, in Matrix3x4 rootxform, boneComputed.MarkBone(bone); } - private void AddSequenceLayers(Span pos, Span q, MStudioSeqDesc seqdesc, int sequence, double cycle, float weight, double time, object? ikContext) { + private void AddSequenceLayers(Span pos, Span q, MStudioSeqDesc seqdesc, int sequence, double cycle, float weight, double time, IKContext? ikContext) { for (int i = 0; i < seqdesc.NumAutoLayers; i++) { MStudioAutoLayer pLayer = seqdesc.Autolayer(i); @@ -1531,7 +1539,12 @@ public static float Studio_GetMass(StudioHdr? studioHdr) { return studioHdr.Mass(); } - public void CalcAutoplaySequences(Span pos, Span q, TimeUnit_t realTime, object? ikContext) { + public void CalcAutoplaySequences(Span pos, Span q, TimeUnit_t realTime, IKContext? ikContext) { + if (studioHdr.GetNumIKChains() == 0 && studioHdr.GetNumIKAutoplayLocks() != 0) + return; + + ikContext?.AddAutoplayLocks(pos, q); + int count = studioHdr.GetAutoplayList(out Span pList); for (int i = 0; i < count; i++) { int sequenceIndex = pList[i]; @@ -1545,6 +1558,8 @@ public void CalcAutoplaySequences(Span pos, Span q, TimeUni AccumulatePose(pos, q, sequenceIndex, cycle, 1.0f, realTime, ikContext); } } + + ikContext?.SolveAutoplayLocks(pos, q); } public static void Studio_BuildMatrices(StudioHdr studioHdr, in QAngle angles, in Vector3 origin, ReadOnlySpan pos, ReadOnlySpan q, int iBone, float flScale, Span bonetoworld, int boneMask) { From 3804a3dfe4062960d4e7c9c59c38ff1db3bb9afb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 21:03:29 +0300 Subject: [PATCH 08/14] Move PhysicsSolidMaskForEntity to shared with client overrides --- Game.Client/C_AI_BaseNPC.cs | 8 ++++++++ Game.Client/C_BasePlayer.cs | 1 + Game.Server/BaseEntity.cs | 2 -- Game.Shared/PhysicsMainShared.cs | 2 ++ 4 files changed, 11 insertions(+), 2 deletions(-) diff --git a/Game.Client/C_AI_BaseNPC.cs b/Game.Client/C_AI_BaseNPC.cs index 4100f4c8e..6a6366adf 100644 --- a/Game.Client/C_AI_BaseNPC.cs +++ b/Game.Client/C_AI_BaseNPC.cs @@ -1,4 +1,5 @@ using Source.Common; +using Source.Common.Formats.BSP; using Source; using Game.Shared; @@ -37,4 +38,11 @@ public class C_AI_BaseNPC : C_BaseCombatCharacter [NetworkName("m_bImportanRagdoll")] public bool ImportantRagdoll; public float TimePingEffect; + + public override Mask PhysicsSolidMaskForEntity() { + if (!IsRagdoll()) + return Mask.NPCSolid; + + return Mask.Solid; + } } diff --git a/Game.Client/C_BasePlayer.cs b/Game.Client/C_BasePlayer.cs index b635bc866..48448b37a 100644 --- a/Game.Client/C_BasePlayer.cs +++ b/Game.Client/C_BasePlayer.cs @@ -96,6 +96,7 @@ public C_BasePlayer() { public int ButtonsArray; public override bool IsPlayer() => true; + public override Mask PhysicsSolidMaskForEntity() => Mask.PlayerSolid; public TimeUnit_t GetFinalPredictedTime() => FinalPredictedTick * TICK_INTERVAL; public bool IsLocalPlayer() => GetLocalPlayer() == this; diff --git a/Game.Server/BaseEntity.cs b/Game.Server/BaseEntity.cs index a3d965ef9..980311b7b 100644 --- a/Game.Server/BaseEntity.cs +++ b/Game.Server/BaseEntity.cs @@ -873,8 +873,6 @@ public bool IsStandable() { public virtual DamageType GetDamageType() => DamageType.Generic; - public virtual Mask PhysicsSolidMaskForEntity() => Mask.Solid; - public BaseEntity? Link; public virtual bool CanBeSeenBy(AI_BaseNPC npc) => true; diff --git a/Game.Shared/PhysicsMainShared.cs b/Game.Shared/PhysicsMainShared.cs index d6ee55eaf..0248d0f0b 100644 --- a/Game.Shared/PhysicsMainShared.cs +++ b/Game.Shared/PhysicsMainShared.cs @@ -387,6 +387,8 @@ public void PhysicsMarkEntitiesAsTouchingEventDriven(BaseEntity other, in Trace link.TouchStamp = TouchLink.TOUCHSTAMP_EVENT_DRIVEN; } + public virtual Mask PhysicsSolidMaskForEntity() => Mask.Solid; + //----------------------------------------------------------------------------- // Purpose: // Input : *other - From 4e02b26d2f49e0d8b4640a91a8093d926c537508 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 21:32:16 +0300 Subject: [PATCH 09/14] Port client entity sphere query --- Game.Client/CDLL_Util.cs | 70 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/Game.Client/CDLL_Util.cs b/Game.Client/CDLL_Util.cs index 0af65e648..5ac6feeaa 100644 --- a/Game.Client/CDLL_Util.cs +++ b/Game.Client/CDLL_Util.cs @@ -5,6 +5,9 @@ using Game.Client; +using Source; +using Source.Common; + using System.Numerics; using System.Runtime.CompilerServices; @@ -56,10 +59,77 @@ public static void Remove(C_BaseEntity? entity) { ClientThinkList().AddToDeleteList(entity.GetClientHandle()); } + public static int EntitiesInSphere(Span list, in Vector3 center, float radius, EntityFlags flagMask, PartitionListMask partitionMask) { + FlaggedEntitiesEnum sphereEnum = new(list, flagMask); + partition.EnumerateElementsInSphere((SpatialPartitionListMask_t)partitionMask, center, radius, false, ref sphereEnum); + + return sphereEnum.GetCount(); + } + } public static partial class Util_Globals { public static bool FStrEq(ReadOnlySpan sz1, ReadOnlySpan sz2) => Unsafe.AreSame(in sz1.DangerousGetReference(), in sz2.DangerousGetReference()) || stricmp(sz1, sz2) == 0; } + + public ref struct FlaggedEntitiesEnum : IPartitionEnumerator + { + public FlaggedEntitiesEnum(Span list, EntityFlags flagMask) { + List = list; + FlagMask = flagMask; + Count = 0; + } + + public int GetCount() => Count; + + public bool AddToList(C_BaseEntity entity) { + if (Count >= List.Length) + return false; + List[Count] = entity; + Count++; + return true; + } + + public IterationRetval EnumElement(IHandleEntity? handleEntity) { + IClientEntity? clientEntity = cl_entitylist.GetClientEntityFromHandle(handleEntity!.GetRefEHandle()); + C_BaseEntity? entity = clientEntity?.GetBaseEntity(); + if (entity != null) { + if (FlagMask != 0 && (entity.GetFlags() & FlagMask) == 0) + return IterationRetval.Continue; + + if (!AddToList(entity)) + return IterationRetval.Stop; + } + + return IterationRetval.Continue; + } + + Span List; + EntityFlags FlagMask; + int Count; + } + + public struct EntitySphereQuery + { + public const int MAX_SPHERE_QUERY = 256; + + public EntitySphereQuery(in Vector3 center, float radius, EntityFlags flagMask = 0, PartitionListMask partitionMask = PartitionListMask.ClientNonStaticEdicts) { + ListIndex = 0; + ListCount = Util.EntitiesInSphere(List, center, radius, flagMask, partitionMask); + } + + public C_BaseEntity? GetCurrentEntity() { + if (ListIndex < ListCount) + return List[ListIndex]; + return null; + } + + public void NextEntity() => ListIndex++; + + [InlineArray(MAX_SPHERE_QUERY)] struct InlineArrayMaxSphereQuery { public T first; } + int ListIndex; + int ListCount; + InlineArrayMaxSphereQuery List; + } } From 6ac709d2b145571af3064c8085268a6754d43357 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 21:48:44 +0300 Subject: [PATCH 10/14] Fix client NPCs not reporting IsNPC --- Game.Client/C_AI_BaseNPC.cs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Game.Client/C_AI_BaseNPC.cs b/Game.Client/C_AI_BaseNPC.cs index 6a6366adf..aee1645ac 100644 --- a/Game.Client/C_AI_BaseNPC.cs +++ b/Game.Client/C_AI_BaseNPC.cs @@ -39,6 +39,8 @@ public class C_AI_BaseNPC : C_BaseCombatCharacter public bool ImportantRagdoll; public float TimePingEffect; + public override bool IsNPC() => true; + public override Mask PhysicsSolidMaskForEntity() { if (!IsRagdoll()) return Mask.NPCSolid; From db12615adcb521521b31eb3668832c44e6775524 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 22:36:12 +0300 Subject: [PATCH 11/14] Port client IK --- Game.Client/C_BaseAnimating.cs | 42 ++++- Game.Client/C_BaseAnimatingOverlay.cs | 2 +- Game.Shared/BaseAnimatingShared.cs | 252 ++++++++++++++++++++++++++ 3 files changed, 292 insertions(+), 4 deletions(-) create mode 100644 Game.Shared/BaseAnimatingShared.cs diff --git a/Game.Client/C_BaseAnimating.cs b/Game.Client/C_BaseAnimating.cs index aa7788d0e..4dd83a709 100644 --- a/Game.Client/C_BaseAnimating.cs +++ b/Game.Client/C_BaseAnimating.cs @@ -8,6 +8,7 @@ using Source.Common; using Source.Common.Commands; using Source.Common.Engine; +using Source.Common.Formats.BSP; using Source.Common.Formats.Keyvalues; using Source.Common.Mathematics; @@ -102,6 +103,8 @@ public bool IsSelfAnimating() { } public bool IsSequenceFinished() => SequenceFinished; public override void ResetLatched() { + Ik = null; + base.ResetLatched(); } public bool IsRagdoll() => Ragdoll != null && RenderFX == (byte)RenderFx.Ragdoll; @@ -456,6 +459,24 @@ public void RemoveBaseAnimatingInterpolatedVars() { public bool ComputeHitboxSurroundingBox(out Vector3 vecWorldMins, out Vector3 vecWorldMaxs) => throw new NotImplementedException(); + public override bool GetSoundSpatialization(ref SpatializationInfo info) { + using (AutoAllowBoneAccess boneaccess = new(true, false)) { + if (!base.GetSoundSpatialization(ref info)) + return false; + } + + if (!Unsafe.IsNullRef(ref info.Origin) && IsNPC() && Ik != null) { + info.Origin = GetAbsOrigin(); + + modelinfo.GetModelBounds(GetModel(), out Vector3 mins, out Vector3 maxs); + MathLib.VectorAdd(mins, maxs, out Vector3 center); + MathLib.VectorScale(center, 0.5f, out center); + + info.Origin += center; + } + return true; + } + public override bool SetupBones(Span boneToWorldOut, int maxBones, int boneMask, double currentTime) { if (!boneToWorldOut.IsEmpty && !IsBoneAccessAllowed()) { if (gpGlobals.RealTime >= SetupBones__lastWarning + 1.0f) { @@ -502,6 +523,8 @@ public override bool SetupBones(Span boneToWorldOut, int maxBones, in AddFlag((EntityFlags)EFL.SettingUpBones); + UpdateIKContext(hdr); + Span pos = stackalloc Vector3[Studio.MAXSTUDIOBONES]; Span q = stackalloc Quaternion[Studio.MAXSTUDIOBONES]; memset(pos.Cast(), float.NaN); @@ -509,9 +532,19 @@ public override bool SetupBones(Span boneToWorldOut, int maxBones, in int bonesMaskNeedRecalc = boneMask | oldReadableBones; + if (Ik != null) { + if (Teleported() || IsNoInterpolationFrame()) + Ik.ClearTargets(); + + InitIK(hdr, currentTime, bonesMaskNeedRecalc); + } + StandardBlendingRules(hdr, pos, q, currentTime, bonesMaskNeedRecalc); BoneBitList boneComputed = new(); + if (Ik != null && !IsRagdoll()) + SolveIK(pos, q, BoneAccessor.GetBoneArrayForWrite(), ref boneComputed, currentTime); + BuildTransformations(hdr, pos, q, parentTransform, bonesMaskNeedRecalc, ref boneComputed); RemoveFlag((EntityFlags)EFL.SettingUpBones); @@ -632,10 +665,11 @@ public virtual void StandardBlendingRules(StudioHdr hdr, Span pos, Span TimeUnit_t cycle = GetCycle(); BoneSetup setup = new(hdr, boneMask, poseparam); setup.InitPose(pos, q); - setup.AccumulatePose(pos, q, GetSequence(), cycle, 1.0f, currentTime, null); + setup.AccumulatePose(pos, q, GetSequence(), cycle, 1.0f, currentTime, Ik); MaintainSequenceTransitions(ref setup, cycle, pos, q); AccumulateLayers(ref setup, pos, q, currentTime); - setup.CalcAutoplaySequences(pos, q, currentTime, null); + + setup.CalcAutoplaySequences(pos, q, currentTime, CreateAutoplayIK(hdr, currentTime, boneMask)); if (hdr.NumBoneControllers() != 0) { // todo @@ -709,7 +743,7 @@ private void MaintainSequenceTransitions(ref BoneSetup boneSetup, double cycle, cycle = blend.Cycle + dt * blend.PlaybackRate * GetSequenceCycleRate(boneSetup.GetStudioHdr(), blend.Sequence); cycle = ClampCycle(cycle, IsSequenceLooping(boneSetup.GetStudioHdr(), blend.Sequence)); - boneSetup.AccumulatePose(pos, q, blend.Sequence, cycle, blend.Weight, gpGlobals.CurTime, null); + boneSetup.AccumulatePose(pos, q, blend.Sequence, cycle, blend.Weight, gpGlobals.CurTime, Ik); } } @@ -924,6 +958,8 @@ private static void RecvProxy_Sequence(ref readonly RecvProxyData data, object i } BoneAccessor.Init(CachedBoneData.Base()); + Ik = null; + if (Attachments.Count != hdr.GetNumAttachments()) Attachments.SetSizeInitialized(hdr.GetNumAttachments()); diff --git a/Game.Client/C_BaseAnimatingOverlay.cs b/Game.Client/C_BaseAnimatingOverlay.cs index 501afc2a5..0eec93895 100644 --- a/Game.Client/C_BaseAnimatingOverlay.cs +++ b/Game.Client/C_BaseAnimatingOverlay.cs @@ -184,7 +184,7 @@ public override void AccumulateLayers(ref BoneSetup boneSetup, Span pos if (fWeight > 1) fWeight = 1; - boneSetup.AccumulatePose(pos, q, AnimOverlay[i].Sequence, fCycle, fWeight, currentTime, null); + boneSetup.AccumulatePose(pos, q, AnimOverlay[i].Sequence, fCycle, fWeight, currentTime, Ik); } } } diff --git a/Game.Shared/BaseAnimatingShared.cs b/Game.Shared/BaseAnimatingShared.cs new file mode 100644 index 000000000..9e56e26f7 --- /dev/null +++ b/Game.Shared/BaseAnimatingShared.cs @@ -0,0 +1,252 @@ +#if CLIENT_DLL || GAME_DLL + +using Game.Shared; + +using Source; +using Source.Common; +using Source.Common.Engine; +using Source.Common.Formats.BSP; +using Source.Common.Mathematics; + +using System.Numerics; + +#if CLIENT_DLL +namespace Game.Client +#else +namespace Game.Server +#endif +{ + public partial class +#if CLIENT_DLL + C_BaseAnimating +#else + BaseAnimating +#endif + { + public IKContext? Ik; + + Vector3 GetIKOrigin() { +#if CLIENT_DLL + return GetRenderOrigin(); +#else + return GetAbsOrigin(); +#endif + } + + QAngle GetIKAngles() { +#if CLIENT_DLL + return GetRenderAngles(); +#else + return GetAbsAngles(); +#endif + } + +#if CLIENT_DLL + protected void UpdateIKContext(StudioHdr hdr) { + if (!IsModelScaled()) { + if (Ik == null && hdr.GetNumIKChains() > 0 && (EntClientFlags & EntClientFlags.DontUseIK) == 0) + Ik = new IKContext(); + } + else + Ik = null; + } + + protected void InitIK(StudioHdr hdr, TimeUnit_t currentTime, int boneMask) { + Ik!.Init(hdr, GetIKAngles(), GetIKOrigin(), currentTime, (int)gpGlobals.FrameCount, boneMask); + } +#endif + + protected IKContext? CreateAutoplayIK(StudioHdr hdr, TimeUnit_t currentTime, int boneMask) { + if (hdr.GetNumIKAutoplayLocks() == 0) + return null; + + IKContext autoIK = new(); + autoIK.Init(hdr, GetIKAngles(), GetIKOrigin(), currentTime, 0, boneMask); + return autoIK; + } + +#if CLIENT_DLL + struct TraceFilterSkipNPCsAndPlayers(IHandleEntity? passentity, CollisionGroup collisionGroup) : ITraceFilter + { + TraceFilterSimple Simple = new(passentity, collisionGroup); + + public bool ShouldHitEntity(IHandleEntity serverEntity, Contents contentsMask) { + if (Simple.ShouldHitEntity(serverEntity, contentsMask)) { + BaseEntity? entity = EntityFromEntityHandle(serverEntity); + if (entity == null) + return true; + + if (entity.IsNPC() || entity.IsPlayer()) + return false; + + return true; + } + return false; + } + } + + protected void SolveIK(Span pos, Span q, Span boneToWorld, ref BoneBitList boneComputed, TimeUnit_t currentTime) { + UpdateIKLocks(currentTime); + + Ik!.UpdateTargets(pos, q, boneToWorld, ref boneComputed); + + CalculateIKLocks(currentTime); + Ik.SolveDependencies(pos, q, boneToWorld, ref boneComputed); + } + + public void UpdateIKLocks(TimeUnit_t currentTime) { + if (Ik == null) + return; + + int targetCount = Ik.Target.Count; + if (targetCount == 0) + return; + + for (int i = 0; i < targetCount; i++) { + IKTarget target = Ik.Target[i]; + + if (!target.IsActive()) + continue; + + if (target.GetOwner() != -1) { + BaseEntity? owner = cl_entitylist.GetEnt(target.GetOwner()); + if (owner != null) + target.UpdateOwner(owner.EntIndex(), owner.GetAbsOrigin(), owner.GetAbsAngles()); + } + } + } + + public void CalculateIKLocks(TimeUnit_t currentTime) { + if (Ik == null) + return; + + int targetCount = Ik.Target.Count; + if (targetCount == 0) + return; + + SpatialPartitionListMask_t curSuppressed = partition.GetSuppressedLists(); + partition.SuppressLists((SpatialPartitionListMask_t)PartitionListMask.AllClientEdicts, false); + PushEnableAbsRecomputations(false); + + Ray ray = default; + TraceFilterSkipNPCsAndPlayers traceFilter = new(this, GetCollisionGroup()); + + MathLib.AngleVectors(GetIKAngles(), out _, out _, out Vector3 up); + + for (int i = 0; i < targetCount; i++) { + Trace trace; + IKTarget target = Ik.Target[i]; + + if (!target.IsActive()) + continue; + + switch (target.Type) { + case Studio.IK_GROUND: { + Vector3 estGround; + Vector3 p1, p2; + + estGround = target.Est.Pos - GetIKOrigin(); + estGround = estGround - (estGround * up) * up; + estGround = GetAbsOrigin() + estGround + target.Est.Floor * up; + + MathLib.VectorMA(estGround, target.Est.Height, up, out p1); + MathLib.VectorMA(estGround, -target.Est.Height, up, out p2); + + float r = Math.Max(target.Est.Radius, 1); + + ray.Init(p1, p2, new Vector3(-r, -r, 0), new Vector3(r, r, r * 2)); + enginetrace.TraceRay(ray, PhysicsSolidMaskForEntity(), ref traceFilter, out trace); + + if (trace.Ent != null && trace.Ent.GetMoveType() == Source.MoveType.Push) + target.SetOwner(trace.Ent.EntIndex(), trace.Ent.GetAbsOrigin(), trace.Ent.GetAbsAngles()); + else + target.ClearOwner(); + + if (trace.StartSolid) { + Vector3 tmp = estGround - target.Trace.Closest; + tmp.NormalizeInPlace(); + ray.Init(estGround - tmp * target.Est.Height, estGround, new Vector3(-r, -r, 0), new Vector3(r, r, 1)); + + enginetrace.TraceRay(ray, Mask.Solid, ref traceFilter, out trace); + + if (!trace.StartSolid) { + p1 = trace.EndPos; + MathLib.VectorMA(p1, -target.Est.Height, up, out p2); + ray.Init(p1, p2, new Vector3(-r, -r, 0), new Vector3(r, r, 1)); + + enginetrace.TraceRay(ray, Mask.Solid, ref traceFilter, out trace); + } + } + + if (!trace.StartSolid) { + if (trace.DidHitWorld()) { + const float limit = 0.832f; + float dot = Vector3.Dot(trace.Plane.Normal, up); + if (dot < limit) { + Assert(dot >= 0); + Vector3 diff = trace.Plane.Normal - up * dot; + float d = MathF.Sqrt((1 - limit * limit) / Vector3.Dot(diff, diff)); + trace.Plane.Normal = up * limit + d * diff; + } + target.SetPosWithNormalOffset(trace.EndPos, trace.Plane.Normal); + target.SetNormal(trace.Plane.Normal); + target.SetOnWorld(true); + } + else if (trace.DidHitNonWorldEntity()) { + target.SetPos(trace.EndPos); + target.SetAngles(GetIKAngles()); + } + else + target.IKFailed(); + } + else { + if (!trace.DidHitWorld()) + target.IKFailed(); + else { + target.SetPos(trace.EndPos); + target.SetAngles(GetIKAngles()); + target.SetOnWorld(true); + } + } + } + break; + + case Studio.IK_ATTACHMENT: { + BaseEntity? entity; + float dist = target.Est.Radius; + + for (EntitySphereQuery sphere = new(target.Est.Pos, 64); (entity = sphere.GetCurrentEntity()) != null; sphere.NextEntity()) { + BaseAnimating? anim = entity.GetBaseAnimating(); + if (anim == null) + continue; + + int attachment = anim.LookupAttachment(target.Offset.AttachmentName); + if (attachment <= 0) + continue; + + anim.GetAttachment(attachment, out Vector3 origin, out QAngle angles); + + float d = (target.Est.Pos - origin).Length(); + + if (d >= dist) + continue; + + dist = d; + target.SetPos(origin); + target.SetAngles(angles); + } + + if (dist >= target.Est.Radius) + target.IKFailed(); + } + break; + } + } + + PopEnableAbsRecomputations(); + partition.SuppressLists(curSuppressed, true); + } +#endif + } +} +#endif From d7c26057efdd0ff1198c4c86b876f59d6b42a147 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 23:19:57 +0300 Subject: [PATCH 12/14] Port server IK and step height adjust --- Game.Server/AI_BaseNPC.cs | 1 + Game.Server/BaseAnimating.cs | 178 ++++++++++++++++++++++++++-- Game.Server/BaseAnimatingOverlay.cs | 6 +- Game.Shared/BaseAnimatingShared.cs | 27 ++++- 4 files changed, 199 insertions(+), 13 deletions(-) diff --git a/Game.Server/AI_BaseNPC.cs b/Game.Server/AI_BaseNPC.cs index 09b151355..c7aeec052 100644 --- a/Game.Server/AI_BaseNPC.cs +++ b/Game.Server/AI_BaseNPC.cs @@ -593,6 +593,7 @@ public AI_BaseNPC() { InChoreo = true; SetCollisionGroup(Source.CollisionGroup.NPC); + EnableServerIK(); } public override void PostConstructor(ReadOnlySpan classname) { diff --git a/Game.Server/BaseAnimating.cs b/Game.Server/BaseAnimating.cs index 2c3b9dca9..2f2627f21 100644 --- a/Game.Server/BaseAnimating.cs +++ b/Game.Server/BaseAnimating.cs @@ -5,6 +5,7 @@ using Source.Common.Commands; using Source.Common.DataCache; using Source.Common.Engine; +using Source.Common.Formats.BSP; using Source.Common.Formats.Keyvalues; using Source.Common.Mathematics; @@ -330,10 +331,6 @@ public int LookupSequence(ReadOnlySpan label) { public static readonly ConVar npc_height_adjust = new("npc_height_adjust", "1", FCvar.Archive, "Enable test mode for ik height adjustment"); - public void UpdateStepOrigin() { - // todo - } - public Activity GetSequenceActivity(int sequence) { if (sequence == -1) { return Activity.ACT_INVALID; @@ -569,6 +566,155 @@ public virtual bool BecomeRagdollOnClient(in Vector3 force) { public bool IsRagdoll() => RenderFX == (byte)RenderFx.Ragdoll; + public override void Teleport(Vector3? newPosition, QAngle? newAngles, Vector3? newVelocity) { + base.Teleport(newPosition, newAngles, newVelocity); + Ik?.ClearTargets(); + InitStepHeightAdjust(); + } + + TimeUnit_t IKGroundContactTime; + float IKGroundMinHeight; + float IKGroundMaxHeight; + + float EstIkFloor; + float EstIkOffset; + + public void InitStepHeightAdjust() { + IKGroundContactTime = 0; + IKGroundMinHeight = 0; + IKGroundMaxHeight = 0; + + EstIkFloor = GetAbsOrigin().Z; + EstIkOffset = 0; + } + + public void UpdateStepOrigin() { + if (!npc_height_adjust.GetBool()) { + EstIkOffset = 0; + EstIkFloor = GetLocalOrigin().Z; + return; + } + + if (IKGroundContactTime > 0.2 && IKGroundContactTime > gpGlobals.CurTime - 0.2) { + if ((GetFlags() & (EntityFlags.Fly | EntityFlags.Swim)) == 0 && GetMoveParent() == null && GetGroundEntity() != null && !GetGroundEntity()!.IsMoving()) { + Vector3 toAbs = GetAbsOrigin() - GetLocalOrigin(); + if (toAbs.Z == 0.0) { + AI_BaseNPC? npc = MyNPCPointer(); + float height = 18.0f; + if (npc != null) + height = npc.StepHeight(); + + EstIkFloor = (float)(EstIkFloor * 0.2 + IKGroundMinHeight * 0.8); + + float bias = Math.Clamp((IKGroundMaxHeight - IKGroundMinHeight) - height, 0.0f, height); + EstIkOffset = Math.Clamp(EstIkFloor - GetAbsOrigin().Z, -height + bias, 0.0f); + return; + } + } + } + + EstIkOffset *= 0.5f; + EstIkFloor = GetLocalOrigin().Z; + } + + public override Vector3 GetStepOrigin() { + Vector3 tmp = GetLocalOrigin(); + tmp.Z += EstIkOffset; + return tmp; + } + + struct TraceFilterSkipNPCs(IHandleEntity? passentity, CollisionGroup collisionGroup) : ITraceFilter + { + TraceFilterSimple Simple = new(passentity, collisionGroup); + + public bool ShouldHitEntity(IHandleEntity serverEntity, Contents contentsMask) { + if (Simple.ShouldHitEntity(serverEntity, contentsMask)) { + BaseEntity? entity = EntityFromEntityHandle(serverEntity); + if (entity == null) + return true; + + if (entity.IsNPC()) + return false; + + return true; + } + return false; + } + } + + public virtual void CalculateIKLocks(TimeUnit_t currentTime) { + if (Ik != null) { + Ray ray = default; + TraceFilterSkipNPCs traceFilter = new(this, GetCollisionGroup()); + GetVectors(out _, out _, out Vector3 up); + + float minHeight = float.MaxValue; + float maxHeight = -float.MaxValue; + + for (int i = 0; i < Ik.Target.Count; i++) { + Trace trace; + IKTarget target = Ik.Target[i]; + + if (!target.IsActive()) + continue; + + switch (target.Type) { + case Studio.IK_GROUND: { + Vector3 estGround; + estGround = target.Est.Pos - GetAbsOrigin(); + estGround = estGround - (estGround * up) * up; + estGround = GetAbsOrigin() + estGround + target.Est.Floor * up; + + MathLib.VectorMA(estGround, target.Est.Height, up, out Vector3 p1); + MathLib.VectorMA(estGround, -target.Est.Height, up, out Vector3 p2); + + float r = Math.Max(target.Est.Radius, 1); + + ray.Init(p1, p2, new Vector3(-r, -r, 0), new Vector3(r, r, 1)); + enginetrace.TraceRay(ray, Mask.Solid, ref traceFilter, out trace); + + if (trace.StartSolid) { + ray.Init(target.Trace.Hip, target.Est.Pos, new Vector3(-r, -r, 0), new Vector3(r, r, 1)); + + enginetrace.TraceRay(ray, Mask.Solid, ref traceFilter, out trace); + + p1 = trace.EndPos; + MathLib.VectorMA(p1, -target.Est.Height, up, out p2); + ray.Init(p1, p2, new Vector3(-r, -r, 0), new Vector3(r, r, 1)); + + enginetrace.TraceRay(ray, Mask.Solid, ref traceFilter, out trace); + + if (trace.StartSolid) + break; + } + + if (trace.DidHitWorld()) { + target.SetPosWithNormalOffset(trace.EndPos, trace.Plane.Normal); + target.SetNormal(trace.Plane.Normal); + } + else { + target.SetPos(trace.EndPos); + target.SetAngles(GetAbsAngles()); + } + + if (target.Est.Release < 0.1) { + float height = Vector3.Dot(target.Est.Pos, up); + maxHeight = Math.Max(height, maxHeight); + minHeight = Math.Min(height, minHeight); + } + } + break; + } + } + + if (minHeight != float.MaxValue && maxHeight != -float.MaxValue) { + IKGroundContactTime = gpGlobals.CurTime; + IKGroundMinHeight = minHeight; + IKGroundMaxHeight = maxHeight; + } + } + } + public virtual bool CanBecomeRagdoll() { int ragdollSequence = SelectWeightedSequence(Activity.ACT_DIERAGDOLL); @@ -590,10 +736,14 @@ public virtual void GetSkeleton(StudioHdr? studioHdr, Span pos, Span boneToWorld, int boneMask) { @@ -613,9 +763,19 @@ public virtual void SetupBones(Span boneToWorld, int boneMask) { Span pos = stackalloc Vector3[Studio.MAXSTUDIOBONES]; Span q = stackalloc Quaternion[Studio.MAXSTUDIOBONES]; - Vector3 adjOrigin = GetAbsOrigin(); + Vector3 adjOrigin = GetAbsOrigin() + new Vector3(0, 0, EstIkOffset); - GetSkeleton(studioHdr, pos, q, boneMask); + if (Ik != null) { + BoneBitList boneComputed = new(); + Ik.Init(studioHdr, GetAbsAngles(), adjOrigin, gpGlobals.CurTime, NextIKFrameCounter(), boneMask); + GetSkeleton(studioHdr, pos, q, boneMask); + + Ik.UpdateTargets(pos, q, boneToWorld, ref boneComputed); + CalculateIKLocks(gpGlobals.CurTime); + Ik.SolveDependencies(pos, q, boneToWorld, ref boneComputed); + } + else + GetSkeleton(studioHdr, pos, q, boneMask); BoneSetup.Studio_BuildMatrices(studioHdr, GetAbsAngles(), adjOrigin, pos, q, -1, GetModelScale(), boneToWorld, boneMask); diff --git a/Game.Server/BaseAnimatingOverlay.cs b/Game.Server/BaseAnimatingOverlay.cs index 5688a99c2..23a03cbea 100644 --- a/Game.Server/BaseAnimatingOverlay.cs +++ b/Game.Server/BaseAnimatingOverlay.cs @@ -55,7 +55,7 @@ public override void GetSkeleton(StudioHdr? studioHdr, Span pos, Span layer = stackalloc int[MAX_OVERLAYS]; int i; @@ -71,11 +71,11 @@ public override void GetSkeleton(StudioHdr? studioHdr, Span pos, Span= 0 && layer[i] < AnimOverlay.Count) { AnimationLayerRef pLayer = AnimOverlay[layer[i]]; - boneSetup.AccumulatePose(pos, q, pLayer.Sequence, pLayer.Cycle, pLayer.Weight, gpGlobals.CurTime, null); + boneSetup.AccumulatePose(pos, q, pLayer.Sequence, pLayer.Cycle, pLayer.Weight, gpGlobals.CurTime, Ik); } } - boneSetup.CalcAutoplaySequences(pos, q, gpGlobals.CurTime, null); + boneSetup.CalcAutoplaySequences(pos, q, gpGlobals.CurTime, CreateAutoplayIK(studioHdr, gpGlobals.CurTime, boneMask)); } public void VerifyOrder() { diff --git a/Game.Shared/BaseAnimatingShared.cs b/Game.Shared/BaseAnimatingShared.cs index 9e56e26f7..60e8b2b02 100644 --- a/Game.Shared/BaseAnimatingShared.cs +++ b/Game.Shared/BaseAnimatingShared.cs @@ -25,6 +25,10 @@ public partial class { public IKContext? Ik; +#if !CLIENT_DLL + int IKCounter; +#endif + Vector3 GetIKOrigin() { #if CLIENT_DLL return GetRenderOrigin(); @@ -41,6 +45,14 @@ QAngle GetIKAngles() { #endif } + int NextIKFrameCounter() { +#if CLIENT_DLL + return (int)gpGlobals.FrameCount; +#else + return ++IKCounter; +#endif + } + #if CLIENT_DLL protected void UpdateIKContext(StudioHdr hdr) { if (!IsModelScaled()) { @@ -50,9 +62,22 @@ protected void UpdateIKContext(StudioHdr hdr) { else Ik = null; } +#else + public void EnableServerIK() { + if (Ik == null) { + Ik = new IKContext(); + IKCounter = 0; + } + } + public void DisableServerIK() { + Ik = null; + } +#endif + +#if CLIENT_DLL protected void InitIK(StudioHdr hdr, TimeUnit_t currentTime, int boneMask) { - Ik!.Init(hdr, GetIKAngles(), GetIKOrigin(), currentTime, (int)gpGlobals.FrameCount, boneMask); + Ik!.Init(hdr, GetIKAngles(), GetIKOrigin(), currentTime, NextIKFrameCounter(), boneMask); } #endif From 4ab9364832514662797c6444562b14bc57cf7171 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Wed, 14 Oct 2026 23:41:33 +0300 Subject: [PATCH 13/14] Port server Entity:SetIK --- Game.Shared/GarrysMod/LuaEntity.cs | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/Game.Shared/GarrysMod/LuaEntity.cs b/Game.Shared/GarrysMod/LuaEntity.cs index a3dfa3f96..3b25d65bf 100644 --- a/Game.Shared/GarrysMod/LuaEntity.cs +++ b/Game.Shared/GarrysMod/LuaEntity.cs @@ -1167,7 +1167,25 @@ static int Entity__SetIK(ILuaInterface lua) { return 0; } #else - // todo: Entity:SetIK + [LuaMethod] + static int Entity__SetIK(ILuaInterface lua) { + BaseEntity ent = Get_Entity(1, false)!; + BaseAnimating? animating = ent.GetBaseAnimating(); + if (animating == null) + return 0; + + if (IsQueuedForRemoval(ent)) + return 0; + + if (ent.IsMarkedForDeletion()) + return 0; + + if (lua.GetBool(2)) + animating.EnableServerIK(); + else + animating.DisableServerIK(); + return 0; + } #endif [LuaMethod] From 6b90e8fa64ccf7e7ec83d4902a447761439575da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=CE=B6eh=20Matt?= <5415177+ZehMatt@users.noreply.github.com> Date: Thu, 15 Oct 2026 00:11:08 +0300 Subject: [PATCH 14/14] Add IK foot clearance on stairs --- Game.Server/BaseAnimating.cs | 1 + Game.Shared/BaseAnimatingShared.cs | 69 +++++++++++++++++++++++++++++- Source.Common/BoneSetupIK.cs | 4 ++ 3 files changed, 73 insertions(+), 1 deletion(-) diff --git a/Game.Server/BaseAnimating.cs b/Game.Server/BaseAnimating.cs index 2f2627f21..f7328b2c2 100644 --- a/Game.Server/BaseAnimating.cs +++ b/Game.Server/BaseAnimating.cs @@ -773,6 +773,7 @@ public virtual void SetupBones(Span boneToWorld, int boneMask) { Ik.UpdateTargets(pos, q, boneToWorld, ref boneComputed); CalculateIKLocks(gpGlobals.CurTime); Ik.SolveDependencies(pos, q, boneToWorld, ref boneComputed); + ApplyFootClearance(studioHdr, pos, q, boneToWorld); } else GetSkeleton(studioHdr, pos, q, boneMask); diff --git a/Game.Shared/BaseAnimatingShared.cs b/Game.Shared/BaseAnimatingShared.cs index 60e8b2b02..c32579b7d 100644 --- a/Game.Shared/BaseAnimatingShared.cs +++ b/Game.Shared/BaseAnimatingShared.cs @@ -90,7 +90,6 @@ protected void InitIK(StudioHdr hdr, TimeUnit_t currentTime, int boneMask) { return autoIK; } -#if CLIENT_DLL struct TraceFilterSkipNPCsAndPlayers(IHandleEntity? passentity, CollisionGroup collisionGroup) : ITraceFilter { TraceFilterSimple Simple = new(passentity, collisionGroup); @@ -110,6 +109,73 @@ public bool ShouldHitEntity(IHandleEntity serverEntity, Contents contentsMask) { } } + protected void ApplyFootClearance(StudioHdr hdr, Span pos, Span q, Span boneToWorld) { + if (Ik == null) + return; + + MathLib.AngleVectors(GetIKAngles(), out _, out _, out Vector3 up); + + for (int i = 0; i < Ik.Target.Count; i++) { + IKTarget target = Ik.Target[i]; + if (target.Type != Studio.IK_GROUND || !target.IsActive() || (uint)target.Chain >= 32) + continue; + + MathLib.QuaternionMatrix(target.Est.Q, out Matrix3x4 footpad); + MathLib.VectorRotate(target.Offset.Pos, footpad, out Vector3 ankle); + float ankleHeight = Vector3.Dot(ankle, up); + if (ankleHeight <= 0) + continue; + + Ik.FootClearanceHeight[target.Chain] = ankleHeight; + Ik.FootClearanceRadius[target.Chain] = Math.Max(target.Est.Radius, 1); + Ik.HasFootClearance[target.Chain] = true; + } + + Ray ray = default; + TraceFilterSkipNPCsAndPlayers traceFilter = new(this, GetCollisionGroup()); + + int numChains = Math.Min(hdr.GetNumIKChains(), 32); + for (int i = 0; i < numChains; i++) { + if (!Ik.HasFootClearance[i]) + continue; + + MStudioIKChain chain = hdr.IKChain(i); + if (chain.NumLinks < 3) + continue; + + int foot = chain.Link(2).Bone; + if (foot < 0 || foot >= hdr.NumBones()) + continue; + + float ankleHeight = Ik.FootClearanceHeight[i]; + float r = Ik.FootClearanceRadius[i]; + + MathLib.MatrixPosition(boneToWorld[foot], out Vector3 footPos); + + ray.Init(footPos + up * FootClearanceMaxRaise, footPos - up * ankleHeight, new Vector3(-r, -r, 0), new Vector3(r, r, 1)); + enginetrace.TraceRay(ray, PhysicsSolidMaskForEntity(), ref traceFilter, out Trace trace); + + if (trace.StartSolid || trace.Fraction >= 1.0f) + continue; + + float raise = Vector3.Dot(trace.EndPos, up) + ankleHeight - Vector3.Dot(footPos, up); + if (raise <= FootClearanceTolerance) + continue; + + raise = Math.Min(raise, FootClearanceMaxRaise); + + if (BoneSetup.Studio_SolveIK(chain, footPos + up * raise, boneToWorld)) { + BoneSetup.SolveBone(hdr, chain.Link(2).Bone, boneToWorld, pos, q); + BoneSetup.SolveBone(hdr, chain.Link(1).Bone, boneToWorld, pos, q); + BoneSetup.SolveBone(hdr, chain.Link(0).Bone, boneToWorld, pos, q); + } + } + } + + const float FootClearanceMaxRaise = 18.0f; + const float FootClearanceTolerance = 0.1f; + +#if CLIENT_DLL protected void SolveIK(Span pos, Span q, Span boneToWorld, ref BoneBitList boneComputed, TimeUnit_t currentTime) { UpdateIKLocks(currentTime); @@ -117,6 +183,7 @@ protected void SolveIK(Span pos, Span q, Span bo CalculateIKLocks(currentTime); Ik.SolveDependencies(pos, q, boneToWorld, ref boneComputed); + ApplyFootClearance(GetModelPtr()!, pos, q, boneToWorld); } public void UpdateIKLocks(TimeUnit_t currentTime) { diff --git a/Source.Common/BoneSetupIK.cs b/Source.Common/BoneSetupIK.cs index 2d378d996..b42936ef8 100644 --- a/Source.Common/BoneSetupIK.cs +++ b/Source.Common/BoneSetupIK.cs @@ -677,6 +677,10 @@ public class IKContext public readonly List Target = new(12); + public readonly float[] FootClearanceHeight = new float[32]; + public readonly float[] FootClearanceRadius = new float[32]; + public readonly bool[] HasFootClearance = new bool[32]; + StudioHdr? studioHdr; readonly List> ikChainRule = [];