From 3a240104e7809f1a2282587cfe93fe37e33cdaea Mon Sep 17 00:00:00 2001 From: Javier Segura Date: Sat, 25 Jul 2026 22:14:29 +0200 Subject: [PATCH] [Feature] Add foot IK with analytic two-bone solver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Clips are authored against a flat ground plane; on real terrain feet float or sink. Foot IK (opt-in per entity, default off) samples the ground beneath each configured ankle every frame and adjusts the hip and knee with a closed-form two-bone solver so the foot lands on it. - TwoBoneIK: Holden-style analytic solver — law-of-cosines bend plus aim rotation, axes expressed in each joint's local frame. Composition order is aim-then-bend in local post-multiplication (equivalent to bend-then-aim in world space); the wrong order is invisible whenever hint, chain, and target are coplanar, so tests cover the non-coplanar case. Unreachable targets clamp to full extension; straight chains take the bend plane from a hint. - FootIK: chains described by hip/knee/ankle joint paths (setFootIKChains), resolved once against the skeleton. The ankle's authored height above the clip's ground plane is preserved above the real terrain; corrections beyond 0.5 m are treated as non-ground and ignored. A collinear pose bend (straight leg) falls back to the chain's configurable bendDirection. - Ground sampling defaults to a downward scene ray pick (octree picking) that rejects hits on the character itself or its descendants; games can override per entity with setFootIKGroundQuery (heightfield, navmesh, physics). - Runs after root motion and transitions, before pose composition. Leg joints are assumed unit-scale. Docs: docs/API/UsingFootIK.md --- Sources/UntoldEngine/Animation/FootIK.swift | 289 ++++++++++++++++++ .../UntoldEngine/Animation/TwoBoneIK.swift | 108 +++++++ Sources/UntoldEngine/ECS/Components.swift | 2 + .../Systems/AnimationSystem.swift | 62 ++++ .../AnimationFootIKTests.swift | 258 ++++++++++++++++ docs/API/UsingFootIK.md | 94 ++++++ 6 files changed, 813 insertions(+) create mode 100644 Sources/UntoldEngine/Animation/FootIK.swift create mode 100644 Sources/UntoldEngine/Animation/TwoBoneIK.swift create mode 100644 Tests/UntoldEngineTests/AnimationFootIKTests.swift create mode 100644 docs/API/UsingFootIK.md diff --git a/Sources/UntoldEngine/Animation/FootIK.swift b/Sources/UntoldEngine/Animation/FootIK.swift new file mode 100644 index 000000000..0c4d2c434 --- /dev/null +++ b/Sources/UntoldEngine/Animation/FootIK.swift @@ -0,0 +1,289 @@ +// +// FootIK.swift +// UntoldEngine +// +// Copyright (C) Untold Engine Studios +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +import Foundation +import simd + +// Foot IK: plants feet on real geometry instead of the flat plane the clip +// was authored against. For each configured leg chain (hip → knee → ankle), +// the ground is sampled beneath the animated ankle, the ankle's authored +// height above the clip's ground plane is preserved above the real terrain, +// and the two-bone solver adjusts the hip and knee so the foot lands there. +// Runs after root motion and transitions, before the pose is composed for +// skinning. Joint scales are assumed uniform (1) along the leg chains. + +/// A world-space ground sample beneath a foot. +public struct FootIKGroundSample { + public var height: Float + public var normal: simd_float3 + + public init(height: Float, normal: simd_float3 = simd_float3(0, 1, 0)) { + self.height = height + self.normal = normal + } +} + +/// Returns the ground beneath a world position, or nil when there is no +/// ground to plant on (the foot is left as authored that frame). +public typealias FootIKGroundQuery = (simd_float3) -> FootIKGroundSample? + +/// One leg chain, identified by skeleton joint paths. +public struct FootIKChainDescriptor { + public var hipPath: String + public var kneePath: String + public var anklePath: String + + /// Extra world-space offset added above the sampled ground, for rigs + /// whose clips are not authored with the ground at model height 0. + public var footHeight: Float + + /// Model-space direction the knee bows toward when the leg is straight + /// and the pose gives no bend to follow (default: character forward). + public var bendDirection: simd_float3 + + public init( + hipPath: String, + kneePath: String, + anklePath: String, + footHeight: Float = 0, + bendDirection: simd_float3 = simd_float3(0, 0, 1) + ) { + self.hipPath = hipPath + self.kneePath = kneePath + self.anklePath = anklePath + self.footHeight = footHeight + self.bendDirection = bendDirection + } +} + +/// Per-entity foot IK state. +struct FootIKState { + var isEnabled = false + var descriptors: [FootIKChainDescriptor] = [] + + /// Corrections larger than this are ignored — a sample this far from + /// the animated foot is a wall, a ledge, or a bad probe, not ground. + var maxAdjustment: Float = 0.5 + + /// Custom ground provider; nil uses the scene ray-picking systems. + var groundQuery: FootIKGroundQuery? + + /// Joint indices resolved against the skeleton; nil until first use or + /// after reconfiguration. Chains with unresolvable paths are dropped. + var resolvedChains: [(hip: Int, knee: Int, ankle: Int, footHeight: Float, bendDirection: simd_float3)]? + + /// Forward-kinematics scratch, reused across frames. + var jointPositions: [simd_float3] = [] + var jointRotations: [simd_quatf] = [] + + mutating func invalidateResolution() { + resolvedChains = nil + } + + /// Refreshes the FK scratch from a pose. Lives on the state so the + /// caller opens a single exclusive access on the component property + /// (two separate `&footIK.x` arguments would overlap and trap). + mutating func refreshForwardKinematics(pose: PoseBuffer, parentIndices: [Int?]) { + computeForwardKinematics( + pose: pose, + parentIndices: parentIndices, + positions: &jointPositions, + rotations: &jointRotations + ) + } +} + +// MARK: - Forward kinematics + +/// Computes model-space joint positions and rotations from a local pose. +/// Scale is ignored — IK chains are assumed unit-scale. Parents must +/// precede children in joint order (same contract as pose composition). +func computeForwardKinematics( + pose: PoseBuffer, + parentIndices: [Int?], + positions: inout [simd_float3], + rotations: inout [simd_quatf] +) { + let jointCount = pose.jointCount + if positions.count != jointCount { + positions = [simd_float3](repeating: .zero, count: jointCount) + rotations = [simd_quatf](repeating: simd_quatf(ix: 0, iy: 0, iz: 0, r: 1), count: jointCount) + } + + for index in 0 ..< jointCount { + if let parentIndex = parentIndices[index] { + positions[index] = positions[parentIndex] + rotations[parentIndex].act(pose.translations[index]) + rotations[index] = simd_normalize(rotations[parentIndex] * pose.rotations[index]) + } else { + positions[index] = pose.translations[index] + rotations[index] = pose.rotations[index] + } + } +} + +// MARK: - Per-frame application + +/// Plants each configured foot chain on the sampled ground by adjusting the +/// hip and knee local rotations in `animationComponent.localPose`. +func applyFootIK( + entityId: EntityID, + animationComponent: AnimationComponent, + skeleton: Skeleton +) { + guard animationComponent.footIK.isEnabled else { return } + + let chains = resolveFootIKChains(state: &animationComponent.footIK, skeleton: skeleton) + guard chains.isEmpty == false else { return } + + let pose = animationComponent.localPose + guard pose.jointCount == skeleton.jointPaths.count else { return } + + animationComponent.footIK.refreshForwardKinematics( + pose: pose, + parentIndices: skeleton.parentIndices + ) + + let worldMatrix = scene.get(component: WorldTransformComponent.self, for: entityId)?.space ?? .identity + let inverseWorldMatrix = worldMatrix.inverse + let maxAdjustment = animationComponent.footIK.maxAdjustment + + for chain in chains { + guard chain.hip < pose.jointCount, chain.knee < pose.jointCount, chain.ankle < pose.jointCount else { + continue + } + + let positions = animationComponent.footIK.jointPositions + let rotations = animationComponent.footIK.jointRotations + + let ankleModel = positions[chain.ankle] + let ankleWorld4 = worldMatrix * simd_float4(ankleModel, 1) + let ankleWorld = simd_float3(ankleWorld4.x, ankleWorld4.y, ankleWorld4.z) + + guard let ground = sampleGround( + at: ankleWorld, + state: animationComponent.footIK, + excluding: entityId + ) else { continue } + + // Preserve the ankle's authored height above the clip's ground + // plane (model height 0) above the real terrain. + let desiredWorldHeight = ground.height + ankleModel.y + chain.footHeight + let correction = desiredWorldHeight - ankleWorld.y + guard abs(correction) > 1e-5, abs(correction) <= maxAdjustment else { continue } + + let targetWorld = ankleWorld + simd_float3(0, correction, 0) + let targetModel4 = inverseWorldMatrix * simd_float4(targetWorld, 1) + let targetModel = simd_float3(targetModel4.x, targetModel4.y, targetModel4.z) + + // Bow the knee the way the clip already bends it. A hint that is + // collinear with the chain (a perfectly straight leg) is useless — + // fall back to the chain's configured bend direction. + let hipPosition = positions[chain.hip] + let kneePosition = positions[chain.knee] + let anklePosition = positions[chain.ankle] + let chainDirection = anklePosition - hipPosition + var bendHint = kneePosition - (hipPosition + anklePosition) * 0.5 + if simd_length_squared(simd_cross(chainDirection, bendHint)) < 1e-8 { + bendHint = chain.bendDirection + } + + var hipLocal = animationComponent.localPose.rotations[chain.hip] + var kneeLocal = animationComponent.localPose.rotations[chain.knee] + solveTwoBoneIK( + a: hipPosition, + b: kneePosition, + c: anklePosition, + target: targetModel, + bendHint: bendHint, + aGlobalRotation: rotations[chain.hip], + bGlobalRotation: rotations[chain.knee], + aLocalRotation: &hipLocal, + bLocalRotation: &kneeLocal + ) + animationComponent.localPose.rotations[chain.hip] = hipLocal + animationComponent.localPose.rotations[chain.knee] = kneeLocal + } +} + +// MARK: - Private helpers + +private func resolveFootIKChains( + state: inout FootIKState, + skeleton: Skeleton +) -> [(hip: Int, knee: Int, ankle: Int, footHeight: Float, bendDirection: simd_float3)] { + if let resolved = state.resolvedChains { + return resolved + } + + var resolved: [(hip: Int, knee: Int, ankle: Int, footHeight: Float, bendDirection: simd_float3)] = [] + for descriptor in state.descriptors { + guard let hip = skeleton.jointPaths.firstIndex(of: descriptor.hipPath), + let knee = skeleton.jointPaths.firstIndex(of: descriptor.kneePath), + let ankle = skeleton.jointPaths.firstIndex(of: descriptor.anklePath) + else { continue } + resolved.append(( + hip: hip, knee: knee, ankle: ankle, + footHeight: descriptor.footHeight, + bendDirection: descriptor.bendDirection + )) + } + + state.resolvedChains = resolved + return resolved +} + +private func sampleGround( + at worldPosition: simd_float3, + state: FootIKState, + excluding entityId: EntityID +) -> FootIKGroundSample? { + if let query = state.groundQuery { + return query(worldPosition) + } + return footIKDefaultGroundSample(at: worldPosition, excluding: entityId) +} + +/// Default ground provider: a downward scene ray pick from above the foot, +/// rejecting hits on the character itself (or its scenegraph descendants). +func footIKDefaultGroundSample( + at worldPosition: simd_float3, + excluding entityId: EntityID +) -> FootIKGroundSample? { + let probeUp: Float = 1.0 + let probeDown: Float = 3.0 + + let origin = worldPosition + simd_float3(0, probeUp, 0) + let options = ScenePickOptions(maxDistance: probeUp + probeDown) + guard let hit = pickEntity(rayOrigin: origin, rayDirection: simd_float3(0, -1, 0), options: options) else { + return nil + } + + guard hit.entityId != entityId, isScenegraphDescendant(hit.entityId, of: entityId) == false else { + return nil + } + + return FootIKGroundSample(height: hit.worldPosition.y, normal: hit.worldNormal ?? simd_float3(0, 1, 0)) +} + +private func isScenegraphDescendant(_ candidate: EntityID, of ancestor: EntityID) -> Bool { + var current = candidate + var steps = 0 + while let scenegraph = scene.get(component: ScenegraphComponent.self, for: current), + scenegraph.parent != .invalid, + steps < 128 + { + if scenegraph.parent == ancestor { + return true + } + current = scenegraph.parent + steps += 1 + } + return false +} diff --git a/Sources/UntoldEngine/Animation/TwoBoneIK.swift b/Sources/UntoldEngine/Animation/TwoBoneIK.swift new file mode 100644 index 000000000..d4f7c11c5 --- /dev/null +++ b/Sources/UntoldEngine/Animation/TwoBoneIK.swift @@ -0,0 +1,108 @@ +// +// TwoBoneIK.swift +// UntoldEngine +// +// Copyright (C) Untold Engine Studios +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +import Foundation +import simd + +// Analytic two-bone inverse kinematics (Daniel Holden, "Simple Two Joint +// IK"): given a chain of three joints — hip a, knee b, ankle c — and a +// target t, adjust the local rotations of a and b so c lands on t. Closed +// form, no iteration: the knee angle comes from the law of cosines and the +// hip is rotated to aim the chain, both applied about axes expressed in +// each joint's own local space. + +/// Solves the two-bone chain in place. +/// +/// - `a`, `b`, `c`: current model-space positions of hip, knee, ankle. +/// - `target`: desired model-space ankle position (clamped to reach). +/// - `bendHint`: model-space direction the knee should bow toward when the +/// chain is fully straight and the bend plane is ambiguous. +/// - `aGlobalRotation`/`bGlobalRotation`: current model-space rotations of +/// hip and knee. +/// - `aLocalRotation`/`bLocalRotation`: local rotations to adjust. +func solveTwoBoneIK( + a: simd_float3, + b: simd_float3, + c: simd_float3, + target: simd_float3, + bendHint: simd_float3, + aGlobalRotation: simd_quatf, + bGlobalRotation: simd_quatf, + aLocalRotation: inout simd_quatf, + bLocalRotation: inout simd_quatf +) { + let epsilon: Float = 1e-5 + + let upperLength = simd_length(b - a) + let lowerLength = simd_length(c - b) + guard upperLength > epsilon, lowerLength > epsilon else { return } + + let targetLength = simd_clamp( + simd_length(target - a), + epsilon, + upperLength + lowerLength - epsilon + ) + + let currentAC = simd_normalize(c - a) + let currentAB = simd_normalize(b - a) + let currentBA = simd_normalize(a - b) + let currentBC = simd_normalize(c - b) + let toTarget = simd_normalize(target - a) + + let angleACAB0 = acos(simd_clamp(simd_dot(currentAC, currentAB), -1, 1)) + let angleBABC0 = acos(simd_clamp(simd_dot(currentBA, currentBC), -1, 1)) + let angleACAT0 = acos(simd_clamp(simd_dot(currentAC, toTarget), -1, 1)) + + let angleACAB1 = acos(simd_clamp( + (lowerLength * lowerLength - upperLength * upperLength - targetLength * targetLength) + / (-2 * upperLength * targetLength), + -1, 1 + )) + let angleBABC1 = acos(simd_clamp( + (targetLength * targetLength - upperLength * upperLength - lowerLength * lowerLength) + / (-2 * upperLength * lowerLength), + -1, 1 + )) + + // Bend-plane axis. When the chain is (nearly) straight the cross + // product vanishes and the plane is ambiguous — fall back to the hint. + var bendAxis = simd_cross(c - a, b - a) + if simd_length_squared(bendAxis) < epsilon { + bendAxis = simd_cross(c - a, bendHint) + } + guard simd_length_squared(bendAxis) > epsilon else { return } + bendAxis = simd_normalize(bendAxis) + + // Aim axis. Vanishes when the chain already points at the target — the + // aim rotation is then zero and any valid axis works. + var aimAxis = simd_cross(c - a, target - a) + if simd_length_squared(aimAxis) < epsilon { + aimAxis = bendAxis + } + aimAxis = simd_normalize(aimAxis) + + // Express the world-space axes in each joint's local space and + // post-multiply onto the local rotations. With both axes converted via + // the ORIGINAL global rotation g, `l * A * B` composes to + // p⁻¹·A_world·B_world·g — so the bend must come last to be applied + // first in world space, then the aim swings the whole chain onto the + // target. (The wrong order is invisible when bend and aim axes + // coincide, which they do whenever hint, chain, and target are + // coplanar — test coverage includes the non-coplanar case.) + let aInverse = aGlobalRotation.inverse + let bInverse = bGlobalRotation.inverse + + let hipBend = simd_quatf(angle: angleACAB1 - angleACAB0, axis: simd_normalize(aInverse.act(bendAxis))) + let kneeBend = simd_quatf(angle: angleBABC1 - angleBABC0, axis: simd_normalize(bInverse.act(bendAxis))) + let hipAim = simd_quatf(angle: angleACAT0, axis: simd_normalize(aInverse.act(aimAxis))) + + aLocalRotation = simd_normalize(aLocalRotation * hipAim * hipBend) + bLocalRotation = simd_normalize(bLocalRotation * kneeBend) +} diff --git a/Sources/UntoldEngine/ECS/Components.swift b/Sources/UntoldEngine/ECS/Components.swift index 8272e296a..08b8dfc48 100644 --- a/Sources/UntoldEngine/ECS/Components.swift +++ b/Sources/UntoldEngine/ECS/Components.swift @@ -204,6 +204,7 @@ public class AnimationComponent: Component { var lastSampleDeltaTime: Float = 0 var transition = PoseTransition() var rootMotion = RootMotionState() + var footIK = FootIKState() public required init() {} @@ -220,6 +221,7 @@ public class AnimationComponent: Component { lastSampleDeltaTime = 0 transition = PoseTransition() rootMotion = RootMotionState() + footIK = FootIKState() } func getAllAnimationClips() -> [String] { diff --git a/Sources/UntoldEngine/Systems/AnimationSystem.swift b/Sources/UntoldEngine/Systems/AnimationSystem.swift index 96c30c35e..cb765d847 100644 --- a/Sources/UntoldEngine/Systems/AnimationSystem.swift +++ b/Sources/UntoldEngine/Systems/AnimationSystem.swift @@ -206,6 +206,14 @@ private func updateAnimationSystem(deltaTime: Float) { to: &animationComponent.localPose, deltaTime: deltaTime ) + // Foot IK corrects the final pose: plant feet on real geometry + // after root motion and transitions have settled the pose. + applyFootIK( + entityId: entity, + animationComponent: animationComponent, + skeleton: skeletonComponent.skeleton + ) + animationComponent.hasSampledPose = true animationComponent.lastSampleDeltaTime = deltaTime @@ -366,6 +374,60 @@ public func setRootMotionEnabled(entityId: EntityID, enabled: Bool, rootJointPat } } +/// Enables or disables foot IK for the entity (or its descendants that +/// carry an `AnimationComponent`). Configure the leg chains first with +/// `setFootIKChains`. +public func setFootIKEnabled(entityId: EntityID, enabled: Bool) { + let animationComponents = animationComponentsForEntityOrDescendants(entityId: entityId) + guard animationComponents.isEmpty == false else { + handleError(.noAnimationComponent, entityId) + return + } + + for (_, animationComponent) in animationComponents { + animationComponent.footIK.isEnabled = enabled + } +} + +public func isFootIKEnabled(entityId: EntityID) -> Bool { + let targetEntityId = resolveEntityWithAnimationComponent(entityId: entityId) ?? entityId + guard let animationComponent = scene.get(component: AnimationComponent.self, for: targetEntityId) else { + handleError(.noAnimationComponent, entityId) + return false + } + + return animationComponent.footIK.isEnabled +} + +/// Configures the leg chains foot IK operates on. Chains whose joint paths +/// do not exist in the skeleton are ignored. +public func setFootIKChains(entityId: EntityID, chains: [FootIKChainDescriptor]) { + let animationComponents = animationComponentsForEntityOrDescendants(entityId: entityId) + guard animationComponents.isEmpty == false else { + handleError(.noAnimationComponent, entityId) + return + } + + for (_, animationComponent) in animationComponents { + animationComponent.footIK.descriptors = chains + animationComponent.footIK.invalidateResolution() + } +} + +/// Overrides how foot IK samples the ground beneath each foot. Pass nil to +/// restore the default scene ray-pick probe. +public func setFootIKGroundQuery(entityId: EntityID, query: FootIKGroundQuery?) { + let animationComponents = animationComponentsForEntityOrDescendants(entityId: entityId) + guard animationComponents.isEmpty == false else { + handleError(.noAnimationComponent, entityId) + return + } + + for (_, animationComponent) in animationComponents { + animationComponent.footIK.groundQuery = query + } +} + public func isRootMotionEnabled(entityId: EntityID) -> Bool { let targetEntityId = resolveEntityWithAnimationComponent(entityId: entityId) ?? entityId guard let animationComponent = scene.get(component: AnimationComponent.self, for: targetEntityId) else { diff --git a/Tests/UntoldEngineTests/AnimationFootIKTests.swift b/Tests/UntoldEngineTests/AnimationFootIKTests.swift new file mode 100644 index 000000000..119103471 --- /dev/null +++ b/Tests/UntoldEngineTests/AnimationFootIKTests.swift @@ -0,0 +1,258 @@ +// +// AnimationFootIKTests.swift +// +// +// Copyright (C) Untold Engine Studios +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +import simd +@testable import UntoldEngine +import XCTest + +@MainActor +final class AnimationFootIKTests: XCTestCase { + var entityId: EntityID! + + private let deltaTime: Float = 1.0 / 90.0 + private let identityRotation = simd_quatf(ix: 0, iy: 0, iz: 0, r: 1) + + // Chain: root at origin, hip at y=0.9, knee at y=0.45, ankle at y=0.1. + // Leg reach = 0.8, fully extended straight down in the rest pose. + private let jointPaths = ["root", "root/hip", "root/hip/knee", "root/hip/knee/ankle"] + private var hipIndex: Int { + 1 + } + + private var ankleIndex: Int { + 3 + } + + override func setUp() async throws { + resetEngineTestState() + + entityId = createEntity() + registerComponent(entityId: entityId, componentType: SkeletonComponent.self) + registerComponent(entityId: entityId, componentType: AnimationComponent.self) + registerComponent(entityId: entityId, componentType: RenderComponent.self) + registerComponent(entityId: entityId, componentType: ScenegraphComponent.self) + registerComponent(entityId: entityId, componentType: LocalTransformComponent.self) + registerComponent(entityId: entityId, componentType: WorldTransformComponent.self) + + let locals = [ + simd_float4x4.identity, + simd_float4x4(translation: simd_float3(0, 0.9, 0)), + simd_float4x4(translation: simd_float3(0, -0.45, 0)), + simd_float4x4(translation: simd_float3(0, -0.35, 0)), + ] + // Bind transforms are model-space accumulations of the locals. + let binds = [ + simd_float4x4.identity, + simd_float4x4(translation: simd_float3(0, 0.9, 0)), + simd_float4x4(translation: simd_float3(0, 0.45, 0)), + simd_float4x4(translation: simd_float3(0, 0.1, 0)), + ] + let runtimeSkeleton = RuntimeSkeleton( + jointPaths: jointPaths, + parentIndices: [nil, 0, 1, 2], + bindTransforms: binds, + restTransforms: locals + ) + scene.get(component: SkeletonComponent.self, for: entityId)?.skeleton = + Skeleton(runtimeSkeleton: runtimeSkeleton) + + // Constant standing pose so foot placement is fully deterministic. + let channels = jointPaths.enumerated().map { index, path in + RuntimeAnimationChannel( + jointPath: path, + translations: [ + .init(time: 0.0, value: localTranslation(of: locals[index])), + .init(time: 1.0, value: localTranslation(of: locals[index])), + ], + rotations: [ + .init(time: 0.0, value: SIMD4(0, 0, 0, 1)), + .init(time: 1.0, value: SIMD4(0, 0, 0, 1)), + ] + ) + } + let clip = AnimationClip(runtimeClip: RuntimeAnimationClip(name: "stand", duration: 1.0, channels: channels)) + + let animationComponent = scene.get(component: AnimationComponent.self, for: entityId)! + animationComponent.animationClips["stand"] = clip + + setFootIKChains(entityId: entityId, chains: [ + FootIKChainDescriptor(hipPath: "root/hip", kneePath: "root/hip/knee", anklePath: "root/hip/knee/ankle"), + ]) + } + + override func tearDown() async throws { + destroyEntity(entityId: entityId) + } + + private func localTranslation(of matrix: simd_float4x4) -> simd_float3 { + simd_float3(matrix.columns.3.x, matrix.columns.3.y, matrix.columns.3.z) + } + + private var animationComponent: AnimationComponent { + scene.get(component: AnimationComponent.self, for: entityId)! + } + + /// Model-space ankle position recomputed from the component's pose. + private func anklePosition() -> simd_float3 { + var positions: [simd_float3] = [] + var rotations: [simd_quatf] = [] + computeForwardKinematics( + pose: animationComponent.localPose, + parentIndices: [nil, 0, 1, 2], + positions: &positions, + rotations: &rotations + ) + return positions[ankleIndex] + } + + private func playOneFrame(groundHeight: Float?) { + setFootIKGroundQuery(entityId: entityId) { _ in + groundHeight.map { FootIKGroundSample(height: $0) } + } + changeAnimation(entityId: entityId, name: "stand", transitionHalflife: 0) + AnimationSystem.shared.update(deltaTime) + } + + // MARK: - Two-bone solver + + private func forwardKinematics( + a: simd_float3, b: simd_float3, c: simd_float3, + aLocal: simd_quatf, bLocal: simd_quatf + ) -> simd_float3 { + let bNew = a + aLocal.act(b - a) + return bNew + (aLocal * bLocal).act(c - b) + } + + func testSolverReachesReachableTarget() { + let a = simd_float3(0, 2, 0) + let b = simd_float3(0.05, 1, 0) + let c = simd_float3(0, 0, 0) + let target = simd_float3(0.5, 0.8, 0) + + var aLocal = identityRotation + var bLocal = identityRotation + solveTwoBoneIK( + a: a, b: b, c: c, target: target, bendHint: simd_float3(0, 0, 1), + aGlobalRotation: identityRotation, bGlobalRotation: identityRotation, + aLocalRotation: &aLocal, bLocalRotation: &bLocal + ) + + let solved = forwardKinematics(a: a, b: b, c: c, aLocal: aLocal, bLocal: bLocal) + XCTAssertLessThan(simd_length(solved - target), 2e-3, "Solved ankle must land on the target") + } + + func testSolverClampsUnreachableTarget() { + let a = simd_float3(0, 2, 0) + let b = simd_float3(0.05, 1, 0) + let c = simd_float3(0, 0, 0) + let reach = simd_length(b - a) + simd_length(c - b) + let target = simd_float3(3, 2, 0) + + var aLocal = identityRotation + var bLocal = identityRotation + solveTwoBoneIK( + a: a, b: b, c: c, target: target, bendHint: simd_float3(0, 0, 1), + aGlobalRotation: identityRotation, bGlobalRotation: identityRotation, + aLocalRotation: &aLocal, bLocalRotation: &bLocal + ) + + let solved = forwardKinematics(a: a, b: b, c: c, aLocal: aLocal, bLocal: bLocal) + XCTAssertEqual(simd_length(solved - a), reach, accuracy: 2e-3, "Chain must extend to full reach") + let direction = simd_normalize(target - a) + let solvedDirection = simd_normalize(solved - a) + XCTAssertLessThan(simd_length(direction - solvedDirection), 2e-3, "Chain must point at the target") + } + + func testSolverHandlesStraightChainWithBendHint() { + let a = simd_float3(0, 2, 0) + let b = simd_float3(0, 1, 0) + let c = simd_float3(0, 0, 0) + let target = simd_float3(0.6, 1.2, 0) + + var aLocal = identityRotation + var bLocal = identityRotation + solveTwoBoneIK( + a: a, b: b, c: c, target: target, bendHint: simd_float3(0, 0, 1), + aGlobalRotation: identityRotation, bGlobalRotation: identityRotation, + aLocalRotation: &aLocal, bLocalRotation: &bLocal + ) + + let solved = forwardKinematics(a: a, b: b, c: c, aLocal: aLocal, bLocal: bLocal) + XCTAssertLessThan(simd_length(solved - target), 2e-3, "Straight chain must still reach via the bend hint") + } + + // MARK: - Foot placement + + func testFootLiftsOntoRaisedGround() { + setFootIKEnabled(entityId: entityId, enabled: true) + playOneFrame(groundHeight: 0.2) + + // Ground at 0.2 plus the ankle's authored height (0.1) above the + // clip's ground plane. + let ankle = anklePosition() + XCTAssertEqual(ankle.y, 0.3, accuracy: 2e-3) + XCTAssertEqual(ankle.x, 0, accuracy: 2e-3) + XCTAssertEqual(ankle.z, 0, accuracy: 2e-3) + } + + func testUnreachableGroundClampsAtFullExtension() { + setFootIKEnabled(entityId: entityId, enabled: true) + // Desired ankle would be at -0.4; the leg (reach 0.8 from hip at + // 0.9) is already fully extended at 0.1 and cannot go lower. + playOneFrame(groundHeight: -0.5) + + XCTAssertEqual(anklePosition().y, 0.1, accuracy: 2e-3, "Fully extended leg cannot reach below full extension") + } + + func testCorrectionBeyondMaxAdjustmentIsIgnored() { + setFootIKEnabled(entityId: entityId, enabled: true) + playOneFrame(groundHeight: 5.0) + + XCTAssertEqual(anklePosition().y, 0.1, accuracy: 1e-4, "A sample far above the foot is not ground; pose must be untouched") + } + + func testDisabledByDefault() { + var queried = false + setFootIKGroundQuery(entityId: entityId) { _ in + queried = true + return FootIKGroundSample(height: 0.2) + } + changeAnimation(entityId: entityId, name: "stand", transitionHalflife: 0) + AnimationSystem.shared.update(deltaTime) + + XCTAssertFalse(isFootIKEnabled(entityId: entityId)) + XCTAssertFalse(queried, "Disabled foot IK must not sample the ground") + XCTAssertEqual(anklePosition().y, 0.1, accuracy: 1e-5) + } + + func testMissingGroundLeavesPoseUntouched() { + setFootIKEnabled(entityId: entityId, enabled: true) + playOneFrame(groundHeight: nil) + + XCTAssertEqual(anklePosition().y, 0.1, accuracy: 1e-5) + } + + func testInvalidChainPathsAreIgnored() { + setFootIKChains(entityId: entityId, chains: [ + FootIKChainDescriptor(hipPath: "no/such", kneePath: "no/such/knee", anklePath: "no/such/ankle"), + ]) + setFootIKEnabled(entityId: entityId, enabled: true) + playOneFrame(groundHeight: 0.2) + + XCTAssertEqual(anklePosition().y, 0.1, accuracy: 1e-5, "Unresolvable chains must be dropped without effect") + } + + func testEnableDisableRoundTrip() { + setFootIKEnabled(entityId: entityId, enabled: true) + XCTAssertTrue(isFootIKEnabled(entityId: entityId)) + setFootIKEnabled(entityId: entityId, enabled: false) + XCTAssertFalse(isFootIKEnabled(entityId: entityId)) + } +} diff --git a/docs/API/UsingFootIK.md b/docs/API/UsingFootIK.md new file mode 100644 index 000000000..4f6c6eebe --- /dev/null +++ b/docs/API/UsingFootIK.md @@ -0,0 +1,94 @@ +# Foot IK + +## Introduction + +Animation clips are authored against a flat ground plane, but game terrain +isn't flat — on slopes and steps, feet float above the ground or sink into +it. **Foot IK** samples the real geometry beneath each foot every frame and +adjusts the leg so the foot lands on it, using an analytic two-bone solver +(hip and knee; no iteration). + +## Why Use It + +- Foot floating/sinking on uneven terrain is the first artifact players + notice on animated characters. +- It composes with root motion: root motion moves the character across the + terrain, foot IK plants each foot on it. +- The solver is closed-form — two joints per leg, a handful of math + operations, no per-frame convergence loop. + +## Step-by-Step Implementation + +Describe each leg as a hip → knee → ankle chain of skeleton joint paths, +then enable: + +```swift +setFootIKChains(entityId: zombie, chains: [ + FootIKChainDescriptor( + hipPath: "root/hips/thigh_l", + kneePath: "root/hips/thigh_l/calf_l", + anklePath: "root/hips/thigh_l/calf_l/foot_l" + ), + FootIKChainDescriptor( + hipPath: "root/hips/thigh_r", + kneePath: "root/hips/thigh_r/calf_r", + anklePath: "root/hips/thigh_r/calf_r/foot_r" + ), +]) +setFootIKEnabled(entityId: zombie, enabled: true) +``` + +By default the ground is found with a downward scene ray pick (the octree +picking system), skipping hits on the character itself. If your game has a +cheaper or more authoritative ground source — a heightfield, a navmesh, a +physics query — plug it in: + +```swift +setFootIKGroundQuery(entityId: zombie) { worldPosition in + guard let height = myTerrain.height(atX: worldPosition.x, z: worldPosition.z) else { + return nil // no ground here: leave the foot as authored + } + return FootIKGroundSample(height: height) +} +``` + +Pass `nil` to restore the default ray probe. + +## What Happens Behind the Scenes + +1. After sampling, root motion, and transitions, the engine computes the + model-space position of each configured ankle. +2. The ground is sampled beneath the ankle (in world space). The ankle's + authored height above the clip's ground plane is preserved above the + real terrain: a foot lifted mid-stride stays lifted. +3. Corrections larger than 0.5 m are ignored — a sample that far from the + animated foot is a ledge or a bad probe, not ground. +4. The two-bone solver rotates the hip and knee so the ankle reaches the + corrected position, bending the knee the way the clip already bends it. + For a perfectly straight leg the pose gives no bend to follow, so the + knee bows toward the chain's `bendDirection` (model space, default + character forward — set it per chain in `FootIKChainDescriptor` if your + rig faces elsewhere). Unreachable targets clamp to full leg extension. +5. The corrected pose is then composed and skinned as usual. + +Chains whose joint paths don't exist in the skeleton are dropped silently — +double-check paths against your rig's joint naming when a leg doesn't +respond. Leg joints are assumed to have unit scale. + +## Tips and Best Practices + +- Feed the query point terrain, not props: with the default ray probe, a + foot over a small rock will step onto it — usually what you want, but a + custom ground query gives you the authority to decide. +- Foot IK adjusts legs only; it does not (yet) lower the pelvis, so a + downhill foot beyond leg reach clamps at full extension rather than + forcing the hips down. +- The ankle's *orientation* is not yet aligned to the ground normal; the + sample's normal is provided for when that lands. + +## Running the Feature + +1. Load a character on uneven ground (a ramp or stairs). +2. Enable foot IK with both leg chains and play a locomotion clip. +3. Toggle `setFootIKEnabled` on and off to compare — watch the feet stop + floating on the high side and stop sinking on the low side.