1use crate::anim::math::lerp3;
2use crate::fx::Comfort;
3use crate::motion::smoothstep;
4use crate::sim::{cosf, expf, sinf};
5
6pub type Vec3 = [f32; 3];
7
8const UP: Vec3 = [0.0, 1.0, 0.0];
9const TAU: f32 = std::f32::consts::TAU;
10
11fn add(a: Vec3, b: Vec3) -> Vec3 {
12 [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
13}
14
15fn sub(a: Vec3, b: Vec3) -> Vec3 {
16 [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
17}
18
19fn scale(a: Vec3, by: f32) -> Vec3 {
20 [a[0] * by, a[1] * by, a[2] * by]
21}
22
23fn length(a: Vec3) -> f32 {
24 (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]).sqrt()
25}
26
27fn finite(value: f32, fallback: f32) -> f32 {
28 if value.is_finite() { value } else { fallback }
29}
30
31fn response(dt: f32, time: f32) -> f32 {
32 if time.is_finite() && time > 1e-6 {
33 1.0 - expf(-dt / time)
34 } else {
35 1.0
36 }
37}
38
39fn damped(from: f32, to: f32, dt: f32, time: f32) -> f32 {
40 from + (to - from) * response(dt, time)
41}
42
43#[derive(Clone, Copy, Debug, PartialEq)]
44pub struct View {
45 pub at: Vec3,
46 pub look_at: Vec3,
47 pub up: Vec3,
48}
49
50impl View {
51 pub fn lerp(self, to: View, t: f32) -> View {
52 View {
53 at: lerp3(self.at, to.at, t),
54 look_at: lerp3(self.look_at, to.look_at, t),
55 up: lerp3(self.up, to.up, t),
56 }
57 }
58
59 pub fn forward(&self) -> Vec3 {
60 let direction = sub(self.look_at, self.at);
61 let long = length(direction);
62 if long > 1e-9 {
63 scale(direction, 1.0 / long)
64 } else {
65 [0.0, 0.0, -1.0]
66 }
67 }
68}
69
70pub trait Probe {
71 fn clear(&self, from: Vec3, direction: Vec3, radius: f32, max: f32) -> f32;
72}
73
74#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
75pub struct Open;
76
77impl Probe for Open {
78 fn clear(&self, _from: Vec3, _direction: Vec3, _radius: f32, max: f32) -> f32 {
79 max
80 }
81}
82
83#[derive(Clone, Copy, Debug, PartialEq)]
84pub struct Input {
85 pub target: Vec3,
86 pub look: [f32; 2],
87 pub mouse: [f32; 2],
88 pub grounded: bool,
89}
90
91impl Input {
92 pub fn at(target: Vec3) -> Self {
93 Self {
94 target,
95 look: [0.0; 2],
96 mouse: [0.0; 2],
97 grounded: true,
98 }
99 }
100
101 pub fn looking(mut self, look: [f32; 2]) -> Self {
102 self.look = look;
103 self
104 }
105
106 pub fn mouse(mut self, mouse: [f32; 2]) -> Self {
107 self.mouse = mouse;
108 self
109 }
110
111 pub fn airborne(mut self) -> Self {
112 self.grounded = false;
113 self
114 }
115}
116
117#[derive(Clone, Copy, Debug, PartialEq)]
118pub struct Bounds {
119 pub min: Vec3,
120 pub max: Vec3,
121}
122
123impl Bounds {
124 pub fn new(min: Vec3, max: Vec3) -> Self {
125 Self { min, max }
126 }
127
128 pub fn clamp(&self, point: Vec3, margin: Vec3) -> Vec3 {
129 std::array::from_fn(|axis| {
130 let low = self.min[axis] + margin[axis];
131 let high = self.max[axis] - margin[axis];
132 if low > high {
133 (low + high) * 0.5
134 } else {
135 point[axis].clamp(low, high)
136 }
137 })
138 }
139}
140
141#[derive(Clone, Copy, Debug, PartialEq)]
142pub struct FollowTuning {
143 pub plane: bool,
144 pub offset: Vec3,
145 pub arm: Vec3,
146 pub dead_zone: Vec3,
147 pub look_ahead: f32,
148 pub look_ahead_speed: f32,
149 pub look_ahead_damping: f32,
150 pub damping: Vec3,
151 pub bounds: Option<Bounds>,
152 pub handover: f32,
153 pub pixels_per_unit: Option<f32>,
154}
155
156impl FollowTuning {
157 pub fn follow3() -> Self {
158 Self {
159 plane: false,
160 offset: [0.0; 3],
161 arm: [0.0, 4.0, 8.0],
162 dead_zone: [0.0; 3],
163 look_ahead: 0.0,
164 look_ahead_speed: 4.0,
165 look_ahead_damping: 0.25,
166 damping: [0.2; 3],
167 bounds: None,
168 handover: 0.6,
169 pixels_per_unit: None,
170 }
171 }
172
173 pub fn follow2() -> Self {
174 Self {
175 plane: true,
176 arm: [0.0, 0.0, 10.0],
177 ..Self::follow3()
178 }
179 }
180}
181
182#[derive(Clone, Copy, Debug, PartialEq)]
183struct Handover {
184 from: Vec3,
185 elapsed: f32,
186}
187
188#[derive(Clone, Debug, PartialEq)]
189pub struct Follow {
190 tuning: FollowTuning,
191 margin: Vec3,
192 focus: Vec3,
193 previous: Vec3,
194 ahead: Vec3,
195 last: Option<Vec3>,
196 started: bool,
197 handover: Option<Handover>,
198}
199
200impl Follow {
201 pub fn new(tuning: FollowTuning) -> Self {
202 Self {
203 tuning,
204 margin: [0.0; 3],
205 focus: [0.0; 3],
206 previous: [0.0; 3],
207 ahead: [0.0; 3],
208 last: None,
209 started: false,
210 handover: None,
211 }
212 }
213
214 pub fn started(&self) -> bool {
215 self.started
216 }
217
218 pub fn tuning(&self) -> &FollowTuning {
219 &self.tuning
220 }
221
222 pub fn tuning_mut(&mut self) -> &mut FollowTuning {
223 &mut self.tuning
224 }
225
226 pub fn focus(&self) -> Vec3 {
227 self.focus
228 }
229
230 pub fn set_view_half(&mut self, half: [f32; 2]) {
231 self.margin = [half[0].max(0.0), half[1].max(0.0), 0.0];
232 }
233
234 pub fn hand_over(&mut self) {
235 self.last = None;
236 if !self.started {
237 return;
238 }
239 if self.tuning.handover > 0.0 {
240 self.handover = Some(Handover {
241 from: self.focus,
242 elapsed: 0.0,
243 });
244 } else {
245 self.started = false;
246 }
247 }
248
249 pub fn restart(&mut self) {
250 self.started = false;
251 self.handover = None;
252 self.last = None;
253 }
254
255 fn bounded(&self, point: Vec3) -> Vec3 {
256 match &self.tuning.bounds {
257 Some(bounds) => bounds.clamp(point, self.margin),
258 None => point,
259 }
260 }
261
262 pub fn tick(&mut self, dt: f32, input: &Input, comfort: Comfort) {
263 let tuning = self.tuning;
264 let target = add(input.target, tuning.offset);
265 if !self.started {
266 self.focus = self.bounded(target);
267 self.previous = self.focus;
268 self.ahead = [0.0; 3];
269 self.last = Some(input.target);
270 self.started = true;
271 self.handover = None;
272 return;
273 }
274 let mut velocity = match self.last {
275 Some(last) if dt > 0.0 => scale(sub(input.target, last), 1.0 / dt),
276 _ => [0.0; 3],
277 };
278 self.last = Some(input.target);
279 if tuning.plane {
280 velocity[2] = 0.0;
281 }
282 let speed = length(velocity);
283 let amount = comfort.amount();
284 let wanted = if tuning.look_ahead > 0.0 && amount > 0.0 && speed > 1e-4 {
285 let reach = (speed / tuning.look_ahead_speed.max(1e-4)).min(1.0);
286 scale(velocity, tuning.look_ahead * amount * reach / speed)
287 } else {
288 [0.0; 3]
289 };
290 let follow = response(dt, tuning.look_ahead_damping);
291 self.ahead = std::array::from_fn(|axis| {
292 self.ahead[axis] + (wanted[axis] - self.ahead[axis]) * follow
293 });
294 let desired = add(target, self.ahead);
295 self.previous = self.focus;
296 match &mut self.handover {
297 Some(handover) => {
298 handover.elapsed += dt;
299 let t = (handover.elapsed / tuning.handover.max(1e-6)).min(1.0);
300 let from = handover.from;
301 self.focus = lerp3(from, desired, smoothstep(t));
302 if t >= 1.0 {
303 self.handover = None;
304 }
305 }
306 None => {
307 for (axis, &wanted) in desired.iter().enumerate() {
308 let gap = wanted - self.focus[axis];
309 let zone = tuning.dead_zone[axis].max(0.0);
310 let excess = if gap > zone {
311 gap - zone
312 } else if gap < -zone {
313 gap + zone
314 } else {
315 0.0
316 };
317 self.focus[axis] +=
318 excess * response(dt, finite(tuning.damping[axis], 0.0).max(0.0));
319 }
320 }
321 }
322 if tuning.plane {
323 self.focus[2] = target[2];
324 self.previous[2] = target[2];
325 }
326 self.focus = self.bounded(self.focus);
327 }
328
329 pub fn pose(&self, alpha: f32) -> View {
330 let alpha = alpha.clamp(0.0, 1.0);
331 let mut focus = lerp3(self.previous, self.focus, alpha);
332 if self.tuning.plane
333 && let Some(pixels) = self.tuning.pixels_per_unit
334 && pixels > 0.0
335 {
336 for axis in focus.iter_mut().take(2) {
337 *axis = (*axis * pixels).round() / pixels;
338 }
339 }
340 View {
341 at: add(focus, self.tuning.arm),
342 look_at: focus,
343 up: UP,
344 }
345 }
346}
347
348#[derive(Clone, Copy, Debug, PartialEq)]
349pub struct Bob {
350 pub stride: f32,
351 pub vertical: f32,
352 pub sway: f32,
353 pub full_speed: f32,
354 pub ease: f32,
355}
356
357impl Default for Bob {
358 fn default() -> Self {
359 Self {
360 stride: 2.2,
361 vertical: 0.04,
362 sway: 0.02,
363 full_speed: 5.0,
364 ease: 0.12,
365 }
366 }
367}
368
369#[derive(Clone, Copy, Debug, PartialEq)]
370pub struct Third {
371 pub distance: f32,
372 pub radius: f32,
373 pub margin: f32,
374 pub min_distance: f32,
375 pub recover: f32,
376 pub shoulder: [f32; 2],
377}
378
379impl Default for Third {
380 fn default() -> Self {
381 Self {
382 distance: 4.0,
383 radius: 0.25,
384 margin: 0.05,
385 min_distance: 0.4,
386 recover: 0.35,
387 shoulder: [0.0; 2],
388 }
389 }
390}
391
392#[derive(Clone, Copy, Debug, PartialEq)]
393pub struct PersonTuning {
394 pub eye: Vec3,
395 pub yaw: f32,
396 pub pitch: f32,
397 pub mouse: f32,
398 pub stick: f32,
399 pub invert_x: bool,
400 pub invert_y: bool,
401 pub smoothing: f32,
402 pub pitch_range: [f32; 2],
403 pub bob: Option<Bob>,
404 pub third: Option<Third>,
405 pub handover: f32,
406}
407
408impl Default for PersonTuning {
409 fn default() -> Self {
410 Self {
411 eye: [0.0, 1.6, 0.0],
412 yaw: 0.0,
413 pitch: 0.0,
414 mouse: 0.1,
415 stick: 180.0,
416 invert_x: false,
417 invert_y: false,
418 smoothing: 0.0,
419 pitch_range: [-89.0, 89.0],
420 bob: None,
421 third: None,
422 handover: 0.5,
423 }
424 }
425}
426
427#[derive(Clone, Copy, Debug, PartialEq)]
428struct Carry {
429 offset: Vec3,
430 elapsed: f32,
431}
432
433#[derive(Clone, Debug, PartialEq)]
434pub struct Person {
435 tuning: PersonTuning,
436 yaw: f32,
437 pitch: f32,
438 residual: [f32; 2],
439 phase: f32,
440 amplitude: f32,
441 arm: f32,
442 third: bool,
443 last: Option<Vec3>,
444 started: bool,
445 changed: bool,
446 carry: Option<Carry>,
447 previous: View,
448 current: View,
449}
450
451const FLAT: View = View {
452 at: [0.0; 3],
453 look_at: [0.0, 0.0, -1.0],
454 up: UP,
455};
456
457impl Person {
458 pub fn new(tuning: PersonTuning) -> Self {
459 let mut person = Self {
460 yaw: tuning.yaw.to_radians(),
461 pitch: 0.0,
462 tuning,
463 residual: [0.0; 2],
464 phase: 0.0,
465 amplitude: 0.0,
466 arm: 0.0,
467 third: false,
468 last: None,
469 started: false,
470 changed: false,
471 carry: None,
472 previous: FLAT,
473 current: FLAT,
474 };
475 person.pitch = person.clamped_pitch(tuning.pitch.to_radians());
476 person
477 }
478
479 pub fn started(&self) -> bool {
480 self.started
481 }
482
483 pub fn tuning(&self) -> &PersonTuning {
484 &self.tuning
485 }
486
487 pub fn tuning_mut(&mut self) -> &mut PersonTuning {
488 &mut self.tuning
489 }
490
491 pub fn yaw(&self) -> f32 {
492 self.yaw
493 }
494
495 pub fn pitch(&self) -> f32 {
496 self.pitch
497 }
498
499 pub fn arm(&self) -> f32 {
500 self.arm
501 }
502
503 pub fn bob_amplitude(&self) -> f32 {
504 self.amplitude
505 }
506
507 pub fn is_third(&self) -> bool {
508 self.third
509 }
510
511 pub fn set_third(&mut self, third: bool) {
512 self.third = third && self.tuning.third.is_some();
513 }
514
515 pub fn set_look(&mut self, yaw: f32, pitch: f32) {
516 self.yaw = yaw;
517 self.pitch = self.clamped_pitch(pitch);
518 self.residual = [0.0; 2];
519 }
520
521 pub fn hand_over(&mut self) {
522 self.last = None;
523 self.changed = self.started;
524 }
525
526 pub fn restart(&mut self) {
527 self.started = false;
528 self.changed = false;
529 self.carry = None;
530 self.last = None;
531 self.residual = [0.0; 2];
532 }
533
534 fn clamped_pitch(&self, pitch: f32) -> f32 {
535 let [low, high] = self.tuning.pitch_range;
536 let low = low.clamp(-89.9, 89.9).to_radians();
537 let high = high.clamp(-89.9, 89.9).to_radians();
538 pitch.clamp(low.min(high), high.max(low))
539 }
540
541 fn directions(&self) -> (Vec3, Vec3, Vec3) {
542 let (sin_yaw, cos_yaw) = (sinf(self.yaw), cosf(self.yaw));
543 let (sin_pitch, cos_pitch) = (sinf(self.pitch), cosf(self.pitch));
544 let forward = [-sin_yaw * cos_pitch, sin_pitch, -cos_yaw * cos_pitch];
545 let right = [cos_yaw, 0.0, -sin_yaw];
546 let up = [
547 right[1] * forward[2] - right[2] * forward[1],
548 right[2] * forward[0] - right[0] * forward[2],
549 right[0] * forward[1] - right[1] * forward[0],
550 ];
551 (forward, right, up)
552 }
553
554 pub fn tick(&mut self, dt: f32, input: &Input, comfort: Comfort, probe: &dyn Probe) {
555 let tuning = self.tuning;
556 let sign = |inverted: bool| if inverted { -1.0 } else { 1.0 };
557 let turn = [
558 -(input.mouse[0] * tuning.mouse + input.look[0] * tuning.stick * dt)
559 * sign(tuning.invert_x),
560 (-input.mouse[1] * tuning.mouse + input.look[1] * tuning.stick * dt)
561 * sign(tuning.invert_y),
562 ];
563 let smoothing = response(dt, tuning.smoothing);
564 let mut steps = [0.0; 2];
565 for ((residual, step), turn) in self.residual.iter_mut().zip(&mut steps).zip(turn) {
566 *residual += finite(turn, 0.0).to_radians();
567 *step = *residual * smoothing;
568 *residual -= *step;
569 }
570 self.yaw += steps[0];
571 self.pitch += steps[1];
572 self.yaw = self.yaw.rem_euclid(TAU);
573 self.pitch = self.clamped_pitch(self.pitch);
574 let planar = match self.last {
575 Some(last) if dt > 0.0 => {
576 let moved = sub(input.target, last);
577 (moved[0] * moved[0] + moved[2] * moved[2]).sqrt() / dt
578 }
579 _ => 0.0,
580 };
581 self.last = Some(input.target);
582 let (forward, right, up) = self.directions();
583 let third = tuning.third.filter(|_| self.third);
584 let reach = tuning.third.map_or(0.0, |third| third.distance.max(0.0));
585 let blend = if reach > 0.0 {
586 (self.arm / reach).clamp(0.0, 1.0)
587 } else {
588 0.0
589 };
590 let mut pivot = add(input.target, tuning.eye);
591 if let Some(third) = third {
592 pivot = add(
593 pivot,
594 add(
595 scale(right, third.shoulder[0] * blend),
596 scale(up, third.shoulder[1] * blend),
597 ),
598 );
599 }
600 match third {
601 Some(third) => {
602 let wanted = third.distance.max(0.0);
603 let back = scale(forward, -1.0);
604 let free = probe
605 .clear(pivot, back, third.radius.max(0.0), wanted + third.margin)
606 .min(wanted + third.margin)
607 - third.margin;
608 let limit = free.max(third.min_distance.min(wanted)).min(wanted);
609 if limit < self.arm {
610 self.arm = limit;
611 } else {
612 self.arm = damped(self.arm, limit, dt, third.recover);
613 }
614 }
615 None => {
616 let recover = tuning.third.map_or(0.0, |third| third.recover);
617 self.arm = damped(self.arm, 0.0, dt, recover);
618 if self.arm < 1e-4 {
619 self.arm = 0.0;
620 }
621 }
622 }
623 let bob = match tuning.bob {
624 Some(bob) if bob.stride > 0.0 => {
625 let moving = input.grounded && planar > 0.05;
626 if moving {
627 self.phase = (self.phase + planar * dt / bob.stride * TAU).rem_euclid(TAU);
628 }
629 let wanted = if moving {
630 (planar / bob.full_speed.max(1e-4)).min(1.0)
631 } else {
632 0.0
633 };
634 self.amplitude = damped(self.amplitude, wanted, dt, bob.ease);
635 let fade = 1.0 - blend;
636 let gain = self.amplitude * comfort.amount() * fade;
637 add(
638 scale(right, cosf(self.phase) * bob.sway * gain),
639 scale(up, sinf(self.phase * 2.0) * bob.vertical * gain),
640 )
641 }
642 _ => {
643 self.amplitude = 0.0;
644 [0.0; 3]
645 }
646 };
647 let mut at = add(add(pivot, scale(forward, -self.arm)), bob);
648 let mut look = add(at, forward);
649 if !self.started {
650 self.started = true;
651 self.carry = None;
652 self.changed = false;
653 let view = View {
654 at,
655 look_at: look,
656 up: UP,
657 };
658 self.previous = view;
659 self.current = view;
660 return;
661 }
662 if self.changed {
663 self.changed = false;
664 if tuning.handover > 0.0 {
665 self.carry = Some(Carry {
666 offset: sub(self.current.at, at),
667 elapsed: 0.0,
668 });
669 } else {
670 self.carry = None;
671 self.current.at = at;
672 }
673 }
674 if let Some(carry) = &mut self.carry {
675 carry.elapsed += dt;
676 let t = (carry.elapsed / tuning.handover.max(1e-6)).min(1.0);
677 let shift = scale(carry.offset, 1.0 - smoothstep(t));
678 at = add(at, shift);
679 look = add(look, shift);
680 if t >= 1.0 {
681 self.carry = None;
682 }
683 }
684 self.previous = self.current;
685 self.current = View {
686 at,
687 look_at: look,
688 up: UP,
689 };
690 }
691
692 pub fn pose(&self, alpha: f32) -> View {
693 self.previous.lerp(self.current, alpha.clamp(0.0, 1.0))
694 }
695}
696
697#[derive(Clone, Debug, PartialEq)]
698pub enum Rig {
699 Follow(Follow),
700 Person(Person),
701}
702
703impl Rig {
704 pub fn tick(&mut self, dt: f32, input: &Input, comfort: Comfort, probe: &dyn Probe) {
705 match self {
706 Rig::Follow(follow) => follow.tick(dt, input, comfort),
707 Rig::Person(person) => person.tick(dt, input, comfort, probe),
708 }
709 }
710
711 pub fn pose(&self, alpha: f32) -> View {
712 match self {
713 Rig::Follow(follow) => follow.pose(alpha),
714 Rig::Person(person) => person.pose(alpha),
715 }
716 }
717
718 pub fn hand_over(&mut self) {
719 match self {
720 Rig::Follow(follow) => follow.hand_over(),
721 Rig::Person(person) => person.hand_over(),
722 }
723 }
724
725 pub fn restart(&mut self) {
726 match self {
727 Rig::Follow(follow) => follow.restart(),
728 Rig::Person(person) => person.restart(),
729 }
730 }
731
732 pub fn started(&self) -> bool {
733 match self {
734 Rig::Follow(follow) => follow.started(),
735 Rig::Person(person) => person.started(),
736 }
737 }
738
739 pub fn follow(&self) -> Option<&Follow> {
740 match self {
741 Rig::Follow(follow) => Some(follow),
742 Rig::Person(_) => None,
743 }
744 }
745
746 pub fn follow_mut(&mut self) -> Option<&mut Follow> {
747 match self {
748 Rig::Follow(follow) => Some(follow),
749 Rig::Person(_) => None,
750 }
751 }
752
753 pub fn person(&self) -> Option<&Person> {
754 match self {
755 Rig::Person(person) => Some(person),
756 Rig::Follow(_) => None,
757 }
758 }
759
760 pub fn person_mut(&mut self) -> Option<&mut Person> {
761 match self {
762 Rig::Person(person) => Some(person),
763 Rig::Follow(_) => None,
764 }
765 }
766}
767
768#[derive(Clone, Copy, Debug, PartialEq)]
769pub struct Blend {
770 from: View,
771 elapsed: f32,
772 duration: f32,
773}
774
775impl Blend {
776 pub fn new(from: View, duration: f32) -> Self {
777 Self {
778 from,
779 elapsed: 0.0,
780 duration,
781 }
782 }
783
784 pub fn step(&mut self, dt: f32) {
785 self.elapsed += dt;
786 }
787
788 pub fn done(&self) -> bool {
789 self.duration <= 0.0 || self.elapsed >= self.duration
790 }
791
792 pub fn apply(&self, to: View) -> View {
793 if self.done() {
794 return to;
795 }
796 self.from.lerp(to, smoothstep(self.elapsed / self.duration))
797 }
798}
799
800#[cfg(test)]
801mod tests;