kvas_engine/
ik.rs

1//! Inverse kinematics on top of a playing clip, for humanoid rigs ([`crate::humanoid`]):
2//!
3//! * **Feet on the ground.** Each foot is traced down onto the real colliders; the hips drop
4//!   for the lower foot and each leg bends (two-bone IK, the knee stays in its plane) so the
5//!   foot stands on stairs, slopes and kerbs instead of floating or sinking. The clip's own
6//!   foot lift (a step) is kept. Needs physics, so it works in the running game.
7//! * **Looking at something.** The chest, neck and head turn (20 / 30 / 50 %) towards a target,
8//!   within a limit, easing in and out; a target behind the back is let go.
9//!
10//! Add [`CharacterIk`] next to an [`crate::skeletal::Animator`]. In a script:
11//! `ctx.look_at_point(npc, Some(player_eyes))` or set `look_at` to an object's name.
12
13use crate::skeletal::{SkinnedModel, Trs};
14use glam::{Mat4, Quat, Vec3};
15use serde::{Deserialize, Serialize};
16
17/// IK settings of an animated humanoid.
18#[derive(Clone, Serialize, Deserialize)]
19#[serde(default)]
20pub struct CharacterIk {
21    /// Plant the feet on the colliders under them.
22    pub feet: bool,
23    /// How far (metres) a foot may be raised or lowered, and the hips dropped.
24    pub max_step: f32,
25    /// Name of the object to look at (empty: none, unless a script gives a point).
26    pub look_at: String,
27    /// 0..1: how much of the turn the body follows.
28    pub look_weight: f32,
29    /// Largest turn away from the clip's own facing, in degrees.
30    pub look_limit: f32,
31    /// A world point to look at, set by scripts (wins over `look_at`; not saved).
32    #[serde(skip)]
33    pub look_point: Option<Vec3>,
34}
35
36impl Default for CharacterIk {
37    fn default() -> Self {
38        Self { feet: true, max_step: 0.45, look_at: String::new(), look_weight: 1.0, look_limit: 75.0, look_point: None }
39    }
40}
41
42impl crate::components::Component for CharacterIk {
43    fn fields(&mut self) -> Vec<crate::reflect::Field<'_>> {
44        use crate::reflect::Field;
45        vec![
46            Field::bool("feet", &mut self.feet),
47            Field::f32("max_step", &mut self.max_step),
48            Field::entity("look_at", &mut self.look_at),
49            Field::f32("look_weight", &mut self.look_weight),
50            Field::f32("look_limit", &mut self.look_limit),
51        ]
52    }
53}
54
55/// What IK remembers between frames (smoothing).
56#[derive(Clone, Default)]
57pub struct IkState {
58    look_dir: Option<Vec3>,
59    look_amount: f32,
60    hips_drop: f32,
61}
62
63/// Rotates a node and everything below it about `pivot`.
64pub(crate) fn rotate_subtree(model: &SkinnedModel, pose: &mut [Trs], root: usize, pivot: Vec3, q: Quat) {
65    let mut stack = vec![root];
66    while let Some(i) = stack.pop() {
67        if let Some(p) = pose.get_mut(i) {
68            p.t = pivot + q * (p.t - pivot);
69            p.r = (q * p.r).normalize();
70        }
71        if let Some(c) = model.children.get(i) {
72            stack.extend(c);
73        }
74    }
75}
76
77fn translate_subtree(model: &SkinnedModel, pose: &mut [Trs], root: usize, d: Vec3) {
78    let mut stack = vec![root];
79    while let Some(i) = stack.pop() {
80        if let Some(p) = pose.get_mut(i) {
81            p.t += d;
82        }
83        if let Some(c) = model.children.get(i) {
84            stack.extend(c);
85        }
86    }
87}
88
89/// Two-bone IK: bends `a`-`b`-`c` (thigh, shin, foot) so `c` reaches `target`, keeping the
90/// knee in the plane it already bends in, and the foot's own orientation.
91fn two_bone(model: &SkinnedModel, pose: &mut [Trs], (a, b, c): (usize, usize, usize), target: Vec3, bend_hint: Vec3) {
92    let (pa, pb, pc) = (pose[a].t, pose[b].t, pose[c].t);
93    let foot_rot = pose[c].r;
94    let (l1, l2) = ((pb - pa).length(), (pc - pb).length());
95    if l1 < 1e-4 || l2 < 1e-4 {
96        return;
97    }
98    let to = target - pa;
99    let d = to.length().clamp((l1 - l2).abs() + 1e-3, l1 + l2 - 1e-3);
100    let n = to.normalize_or_zero();
101    if n == Vec3::ZERO {
102        return;
103    }
104    // Which way the knee points: as it does now, or the hint (forward) for a straight leg.
105    let mut k = (pb - pa) - n * (pb - pa).dot(n);
106    if k.length_squared() < 1e-6 {
107        k = bend_hint - n * bend_hint.dot(n);
108    }
109    let k = k.normalize_or_zero();
110    let cos_a = ((l1 * l1 + d * d - l2 * l2) / (2.0 * l1 * d)).clamp(-1.0, 1.0);
111    let knee = pa + n * (l1 * cos_a) + k * (l1 * (1.0 - cos_a * cos_a).sqrt());
112    let q1 = Quat::from_rotation_arc((pb - pa).normalize(), (knee - pa).normalize());
113    rotate_subtree(model, pose, a, pa, q1);
114    let (nb, nc) = (pose[b].t, pose[c].t);
115    let q2 = Quat::from_rotation_arc((nc - nb).normalize_or_zero(), (pa + n * d - nb).normalize_or_zero());
116    rotate_subtree(model, pose, b, nb, q2);
117    // The foot keeps the clip's orientation.
118    let fix = (foot_rot * pose[c].r.inverse()).normalize();
119    let pc = pose[c].t;
120    rotate_subtree(model, pose, c, pc, fix);
121}
122
123/// Applies IK to a model-space `pose`. `world` is the object's transform; `ground(from, len)`
124/// traces straight down and returns the hit height (world space).
125pub(crate) fn apply(
126    model: &SkinnedModel,
127    pose: &mut [Trs],
128    world: Mat4,
129    ik: &CharacterIk,
130    look_target: Option<Vec3>,
131    ground: Option<&dyn Fn(Vec3, f32) -> Option<f32>>,
132    state: &mut IkState,
133    dt: f32,
134) {
135    let Some(h) = &model.humanoid else { return };
136    let inv = world.inverse();
137    let ease = (dt * 8.0).clamp(0.0, 1.0);
138    // ---- feet
139    if let (true, Some(ground), Some(hips)) = (ik.feet, ground, h.hips) {
140        let mut targets = [None, None];
141        let mut drop = 0.0f32;
142        for s in 0..2 {
143            let (Some(_), Some(_), Some(c)) = (h.thigh[s], h.shin[s], h.foot[s]) else { continue };
144            let rest_height = model.nodes[c].rest.w_axis.y;
145            let foot = pose[c].t;
146            let lift = (foot.y - rest_height).max(0.0);
147            let w = world.transform_point3(foot.with_y(rest_height));
148            let up = world.transform_vector3(h.up).normalize_or(Vec3::Y);
149            let reach = ik.max_step * world.transform_vector3(Vec3::Y).length().max(1e-3);
150            let Some(hit_y) = ground(w + up * reach, reach * 2.0) else { continue };
151            // The ground under this foot, as a height in model space.
152            let g = inv.transform_point3(Vec3::new(w.x, hit_y, w.z)).y.clamp(-ik.max_step, ik.max_step);
153            drop = drop.min(g);
154            targets[s] = Some(Vec3::new(foot.x, g + rest_height + lift, foot.z));
155        }
156        state.hips_drop += (drop - state.hips_drop) * ease;
157        if state.hips_drop.abs() > 1e-4 {
158            translate_subtree(model, pose, hips, h.up * state.hips_drop);
159        }
160        for s in 0..2 {
161            if let (Some(t), Some(a), Some(b), Some(c)) = (targets[s], h.thigh[s], h.shin[s], h.foot[s]) {
162                two_bone(model, pose, (a, b, c), t, h.forward);
163            }
164        }
165    }
166    // ---- look
167    let Some(head) = h.head else { return };
168    let head_pos = pose[head].t;
169    let rest_rot = Trs::from_mat(model.nodes[head].rest).r;
170    let facing = (pose[head].r * rest_rot.inverse() * h.forward).normalize_or(h.forward);
171    let want = look_target.map(|t| (inv.transform_point3(t) - head_pos).normalize_or_zero()).filter(|d| *d != Vec3::ZERO);
172    let wanted = want.filter(|d| d.angle_between(facing).to_degrees() < ik.look_limit + 25.0);
173    let target_amount = if wanted.is_some() { ik.look_weight.clamp(0.0, 1.0) } else { 0.0 };
174    state.look_amount += (target_amount - state.look_amount) * (dt * 4.0).clamp(0.0, 1.0);
175    if let Some(d) = wanted {
176        // Within the limit.
177        let angle = d.angle_between(facing);
178        let limit = ik.look_limit.to_radians();
179        let d = if angle > limit { facing.slerp_to(d, limit / angle) } else { d };
180        let prev = state.look_dir.unwrap_or(facing);
181        state.look_dir = Some(prev.slerp_to(d, (dt * 6.0).clamp(0.0, 1.0)));
182    }
183    if state.look_amount < 1e-3 {
184        return;
185    }
186    let dir = state.look_dir.unwrap_or(facing);
187    let full = Quat::from_rotation_arc(facing, dir);
188    let turn = Quat::IDENTITY.slerp(full, state.look_amount);
189    let mut chain: Vec<(usize, f32)> = [(h.chest, 0.2), (h.neck, 0.3)].into_iter().filter_map(|(b, k)| Some((b?, k))).collect();
190    let used: f32 = chain.iter().map(|c| c.1).sum();
191    chain.push((head, 1.0 - used));
192    for (bone, k) in chain {
193        let pivot = pose[bone].t;
194        rotate_subtree(model, pose, bone, pivot, Quat::IDENTITY.slerp(turn, k));
195    }
196}
197
198trait SlerpTo {
199    fn slerp_to(self, to: Vec3, k: f32) -> Vec3;
200}
201
202impl SlerpTo for Vec3 {
203    fn slerp_to(self, to: Vec3, k: f32) -> Vec3 {
204        Quat::IDENTITY.slerp(Quat::from_rotation_arc(self.normalize_or(Vec3::Z), to.normalize_or(Vec3::Z)), k) * self
205    }
206}
kvas_engine/ik.rs

kvas_engine/
ik.rs
