[Feature] Reach IK: a target per chain, in world, model, shoulder or ground space

Sources/UntoldEngine/Animation/ReachIK.swift

@@ -11,13 +11,16 @@
 import Foundation
 import simd
 
-// Reach IK: bends arm chains (shoulder → elbow → hand) toward a world
+// Reach IK: bends two-bone chains (shoulder → elbow → hand for an arm, hip
+// → knee → ankle for a leg held on a spot) toward a world
 // target with the two-bone solver foot IK uses. A target within reach is
 // touched; one beyond it is pointed at, the arm extended to a fraction of
 // its length along the direction so the elbow never locks. The influence
 // eases in and out, and the solve blends over whatever the pose (or the
 // pose layer) put the arms in, so the elbow keeps the posture's bend. Runs
-// after the pose layer and before foot IK. See docs/API/UsingPoseLayers.md.
+// after the pose layer and before foot IK. Chains share one target (a
+// character grabbing at something) or take one each (hands driven by
+// tracking). See docs/API/UsingPoseLayers.md.
 
 private let ln2: Float = 0.693_147_18
 
@@ -44,6 +47,33 @@ public struct ReachIKChainDescriptor {
     }
 }
 
+/// Where one chain reaches, when the chains do not share a target.
+public struct ReachIKChainTarget: Sendable {
+    public enum Space: Sendable {
+        /// A world position.
+        case world
+        /// A position in the entity's model space.
+        case model
+        /// An offset from the chain's own shoulder, along the model axes:
+        /// the hand keeps its place relative to the body however the body
+        /// moves this frame.
+        case shoulder
+        /// A spot in the entity's model space whose height is ignored:
+        /// the chain's end goes over it at the height the pose gives it
+        /// (a foot held where it stands while the pose lifts and lowers
+        /// it).
+        case modelGround
+    }
+
+    public var position: simd_float3
+    public var space: Space
+
+    public init(position: simd_float3, space: Space = .world) {
+        self.position = position
+        self.space = space
+    }
+}
+
 /// Per-entity reach IK state.
 struct ReachIKState {
     var descriptors: [ReachIKChainDescriptor] = []
@@ -57,6 +87,9 @@ struct ReachIKState {
     /// instead of one. Nil until the first target.
     var smoothedTarget: simd_float3?
     var targetHalflife: Float = 0.08
+    /// The same easing for the chains' own targets: theirs to set, so a
+    /// tracked hand followed exactly does not make the shared target jump.
+    var chainTargetHalflife: Float = 0.08
     var weight: Float = 0
     var targetWeight: Float = 0
     var halflife: Float = 0.25
@@ -65,6 +98,12 @@ struct ReachIKState {
     /// Per-chain multipliers on the influence, index-aligned with the
     /// chains (empty: 1 for all) — one hand lunging while the other holds.
     var chainWeights: [Float] = []
+    /// Per-chain targets, index-aligned with the chains; a chain without
+    /// one reaches for `targetWorld`.
+    var chainTargets: [ReachIKChainTarget?] = []
+    /// The chain targets as the solve uses them, eased like
+    /// `smoothedTarget`, each in its target's own space.
+    var smoothedChainTargets: [ReachIKChainTarget?] = []
 
     var jointPositions: [simd_float3] = []
     var jointRotations: [simd_quatf] = []
@@ -73,6 +112,30 @@ struct ReachIKState {
         resolvedChains = nil
     }
 
+    var hasChainTargets: Bool {
+        chainTargets.contains { $0 != nil }
+    }
+
+    /// The chain's own target eased toward its newest value; a target that
+    /// is new, or changed space, is taken as it is.
+    mutating func easedChainTarget(_ index: Int, ease: Float) -> ReachIKChainTarget? {
+        guard index < chainTargets.count, let target = chainTargets[index] else {
+            if index < smoothedChainTargets.count {
+                smoothedChainTargets[index] = nil
+            }
+            return nil
+        }
+        if smoothedChainTargets.count <= index {
+            smoothedChainTargets += [ReachIKChainTarget?](repeating: nil, count: index + 1 - smoothedChainTargets.count)
+        }
+        var eased = target
+        if let previous = smoothedChainTargets[index], previous.space == target.space {
+            eased.position = previous.position + (target.position - previous.position) * ease
+        }
+        smoothedChainTargets[index] = eased
+        return eased
+    }
+
     mutating func refreshForwardKinematics(pose: PoseBuffer, parentIndices: [Int?]) {
         computeForwardKinematics(
             pose: pose, parentIndices: parentIndices,
@@ -112,19 +175,25 @@ func applyReachIK(
         animationComponent.reachIK.weight = animationComponent.reachIK.targetWeight
     }
     let weight = animationComponent.reachIK.weight
-    guard weight > 1e-4, let rawTarget = animationComponent.reachIK.targetWorld else {
+    let rawTarget = animationComponent.reachIK.targetWorld
+    guard weight > 1e-4, rawTarget != nil || animationComponent.reachIK.hasChainTargets else {
         if animationComponent.reachIK.targetWeight <= 0 {
             animationComponent.reachIK.targetWorld = nil
             animationComponent.reachIK.smoothedTarget = nil
+            animationComponent.reachIK.chainTargets.removeAll()
+            animationComponent.reachIK.smoothedChainTargets.removeAll()
         }
         return
     }
-    let targetWorld: simd_float3
-    if let previous = animationComponent.reachIK.smoothedTarget {
-        let ease = 1 - exp(-ln2 * deltaTime / max(animationComponent.reachIK.targetHalflife, 1e-4))
-        targetWorld = previous + (rawTarget - previous) * ease
-    } else {
-        targetWorld = rawTarget
+    let ease = 1 - exp(-ln2 * deltaTime / max(animationComponent.reachIK.targetHalflife, 1e-4))
+    let chainEase = 1 - exp(-ln2 * deltaTime / max(animationComponent.reachIK.chainTargetHalflife, 1e-4))
+    var targetWorld: simd_float3?
+    if let rawTarget {
+        if let previous = animationComponent.reachIK.smoothedTarget {
+            targetWorld = previous + (rawTarget - previous) * ease
+        } else {
+            targetWorld = rawTarget
+        }
     }
     animationComponent.reachIK.smoothedTarget = targetWorld
 
@@ -138,12 +207,19 @@ func applyReachIK(
     let rotations = animationComponent.reachIK.jointRotations
 
     let worldMatrix = scene.get(component: WorldTransformComponent.self, for: entityId)?.space ?? .identity
-    let targetModel4 = worldMatrix.inverse * simd_float4(targetWorld, 1)
-    let targetModel = simd_float3(targetModel4.x, targetModel4.y, targetModel4.z)
+    let worldToModel = worldMatrix.inverse
+    func modelPosition(_ world: simd_float3) -> simd_float3 {
+        let p = worldToModel * simd_float4(world, 1)
+        return simd_float3(p.x, p.y, p.z)
+    }
+    let targetModel = targetWorld.map(modelPosition)
     let reach = animationComponent.reachIK.reach
     let chainWeights = animationComponent.reachIK.chainWeights
 
     for (index, chain) in chains.enumerated() {
+        // Eased every frame, also while the chain's weight is zero, so
+        // the target is current when the weight returns.
+        let chainTarget = animationComponent.reachIK.easedChainTarget(index, ease: chainEase)
         let chainWeight = weight * (index < chainWeights.count ? min(max(chainWeights[index], 0), 1) : 1)
         guard chainWeight > 1e-4 else { continue }
         guard chain.shoulder < pose.jointCount, chain.elbow < pose.jointCount, chain.hand < pose.jointCount else {
@@ -157,7 +233,19 @@ func applyReachIK(
 
         // Beyond reach the arm points at the target, a little short of
         // straight.
-        var goal = targetModel
+        var goal: simd_float3
+        if let chainTarget {
+            switch chainTarget.space {
+            case .world: goal = modelPosition(chainTarget.position)
+            case .model: goal = chainTarget.position
+            case .shoulder: goal = shoulder + chainTarget.position
+            case .modelGround: goal = simd_float3(chainTarget.position.x, hand.y, chainTarget.position.z)
+            }
+        } else if let targetModel {
+            goal = targetModel
+        } else {
+            continue
+        }
         let toTarget = goal - shoulder
         let distance = simd_length(toTarget)
         guard distance > 1e-4 else { continue }

Sources/UntoldEngine/Systems/AnimationSystem.swift

@@ -606,6 +606,12 @@ public func setReachIKChains(entityId: EntityID, chains: [ReachIKChainDescriptor
 /// reach and points at it beyond, the arm extended to `reach` of its
 /// length. The influence eases to `weight` over `halflife`; a nil position
 /// eases it back out.
+///
+/// An entity has one reach influence: `weight`, `halflife` and `reach` are
+/// shared with `setReachIKChainTargets`, and the call made last sets them
+/// for every chain. A caller that uses both in the same frame (tracked
+/// hands on some chains, this target for the rest) passes the same values
+/// to both.
 public func setReachIKTarget(
     entityId: EntityID,
     worldPosition: simd_float3?,
@@ -634,6 +640,52 @@ public func setReachIKTarget(
     }
 }
 
+/// Gives every reach chain a target of its own, index-aligned with the
+/// chains: hands driven by tracking, each going where its tracked hand
+/// is. A chain whose entry is nil (or missing) reaches for the shared
+/// target of `setReachIKTarget`, or keeps its pose when there is none.
+/// The influence eases to `weight` over `halflife`; with no target at all
+/// it eases back out. Each target is eased over `targetHalflife`, in its
+/// own space; 0 follows it exactly (a source that is already filtered).
+///
+/// An entity has one reach influence: `weight`, `halflife` and `reach` are
+/// shared with `setReachIKTarget`, and the call made last sets them for
+/// every chain, so a caller that mixes the two passes the same values to
+/// both. `targetHalflife` is the chains' own: it does not change how the
+/// shared target is eased.
+public func setReachIKChainTargets(
+    entityId: EntityID,
+    targets: [ReachIKChainTarget?],
+    weight: Float = 1,
+    halflife: Float = 0.25,
+    reach: Float = 0.95,
+    targetHalflife: Float = 0.08
+) {
+    let animationComponents = animationComponentsForEntityOrDescendants(entityId: entityId)
+    guard animationComponents.isEmpty == false else {
+        handleError(.noAnimationComponent, entityId)
+        return
+    }
+
+    for (_, animationComponent) in animationComponents {
+        if targets.contains(where: { $0 != nil }) {
+            animationComponent.reachIK.chainTargets = targets
+            animationComponent.reachIK.targetWeight = min(max(weight, 0), 1)
+        } else if animationComponent.reachIK.targetWorld == nil {
+            // The last targets stay while the influence fades.
+            animationComponent.reachIK.targetWeight = 0
+        } else {
+            animationComponent.reachIK.chainTargets = targets
+        }
+        animationComponent.reachIK.halflife = max(halflife, 0)
+        animationComponent.reachIK.reach = min(max(reach, 0.05), 1)
+        animationComponent.reachIK.chainTargetHalflife = max(targetHalflife, 0)
+        if halflife <= 0 {
+            animationComponent.reachIK.weight = animationComponent.reachIK.targetWeight
+        }
+    }
+}
+
 /// Per-chain multipliers on the reach influence, index-aligned with the
 /// chains: 0 leaves that arm to the pose, 1 gives it the full influence —
 /// one hand lunging while the other holds back.