1use glam::{Quat, Vec3A};
2
3use crate::{BoneIndex, MorphIndex, PoseArena};
4
5const BEZIER_ITERATIONS: usize = 15;
6const BEZIER_EPSILON: f32 = 1.0e-5;
7const MMD_INTERPOLATION_SCALE: f32 = 1.0 / 127.0;
8
9#[derive(Clone, Copy, Debug, PartialEq, Eq)]
10pub struct InterpolationScalar {
11 pub x1: u8,
12 pub y1: u8,
13 pub x2: u8,
14 pub y2: u8,
15}
16
17impl InterpolationScalar {
18 pub const fn linear() -> Self {
19 Self {
20 x1: 20,
21 y1: 20,
22 x2: 107,
23 y2: 107,
24 }
25 }
26
27 pub fn evaluate(self, x: f32) -> f32 {
28 let x = x.clamp(0.0, 1.0);
29 if x <= 0.0 {
30 return 0.0;
31 }
32 if x >= 1.0 {
33 return 1.0;
34 }
35 bezier_interpolation(
36 self.x1 as f32 * MMD_INTERPOLATION_SCALE,
37 self.x2 as f32 * MMD_INTERPOLATION_SCALE,
38 self.y1 as f32 * MMD_INTERPOLATION_SCALE,
39 self.y2 as f32 * MMD_INTERPOLATION_SCALE,
40 x,
41 )
42 }
43}
44
45impl Default for InterpolationScalar {
46 fn default() -> Self {
47 Self::linear()
48 }
49}
50
51#[derive(Clone, Copy, Debug, PartialEq, Eq)]
52pub struct InterpolationVector3 {
53 pub x: InterpolationScalar,
54 pub y: InterpolationScalar,
55 pub z: InterpolationScalar,
56}
57
58impl InterpolationVector3 {
59 pub const fn linear() -> Self {
60 Self {
61 x: InterpolationScalar::linear(),
62 y: InterpolationScalar::linear(),
63 z: InterpolationScalar::linear(),
64 }
65 }
66}
67
68impl Default for InterpolationVector3 {
69 fn default() -> Self {
70 Self::linear()
71 }
72}
73
74#[derive(Clone, Debug)]
75pub struct MovableBoneKeyframe {
76 pub frame: u32,
77 pub position: Vec3A,
78 pub rotation: Quat,
79 pub position_interpolation: InterpolationVector3,
80 pub rotation_interpolation: InterpolationScalar,
81}
82
83impl MovableBoneKeyframe {
84 pub fn new(frame: u32, position: Vec3A, rotation: Quat) -> Self {
85 Self {
86 frame,
87 position,
88 rotation,
89 position_interpolation: InterpolationVector3::linear(),
90 rotation_interpolation: InterpolationScalar::linear(),
91 }
92 }
93}
94
95#[derive(Clone, Debug)]
96pub struct MovableBoneTrack {
97 frame_numbers: Box<[u32]>,
98 positions: Box<[Vec3A]>,
99 rotations: Box<[Quat]>,
100 position_interpolations: Box<[InterpolationVector3]>,
101 rotation_interpolations: Box<[InterpolationScalar]>,
102}
103
104impl MovableBoneTrack {
105 pub fn from_keyframes(mut keyframes: Vec<MovableBoneKeyframe>) -> Self {
106 keyframes.sort_by_key(|keyframe| keyframe.frame);
107
108 let mut frame_numbers = Vec::with_capacity(keyframes.len());
109 let mut positions = Vec::with_capacity(keyframes.len());
110 let mut rotations = Vec::with_capacity(keyframes.len());
111 let mut position_interpolations = Vec::with_capacity(keyframes.len());
112 let mut rotation_interpolations = Vec::with_capacity(keyframes.len());
113
114 for keyframe in keyframes {
115 frame_numbers.push(keyframe.frame);
116 positions.push(keyframe.position);
117 rotations.push(keyframe.rotation.normalize());
118 position_interpolations.push(keyframe.position_interpolation);
119 rotation_interpolations.push(keyframe.rotation_interpolation);
120 }
121
122 Self {
123 frame_numbers: frame_numbers.into_boxed_slice(),
124 positions: positions.into_boxed_slice(),
125 rotations: rotations.into_boxed_slice(),
126 position_interpolations: position_interpolations.into_boxed_slice(),
127 rotation_interpolations: rotation_interpolations.into_boxed_slice(),
128 }
129 }
130
131 pub fn sample(&self, frame: f32) -> Option<(Vec3A, Quat)> {
132 match self.frame_numbers.len() {
133 0 => None,
134 1 => Some((self.positions[0], self.rotations[0])),
135 _ => {
136 let next_index = self.find_next_keyframe(frame);
137 if next_index == 0 {
138 return Some((self.positions[0], self.rotations[0]));
139 }
140 if next_index >= self.frame_numbers.len() {
141 let last = self.frame_numbers.len() - 1;
142 return Some((self.positions[last], self.rotations[last]));
143 }
144
145 let prev_index = next_index - 1;
146 let prev_frame = self.frame_numbers[prev_index] as f32;
147 let next_frame = self.frame_numbers[next_index] as f32;
148 let frame_t = if next_frame == prev_frame {
149 0.0
150 } else {
151 ((frame - prev_frame) / (next_frame - prev_frame)).clamp(0.0, 1.0)
152 };
153
154 let interpolation = self.position_interpolations[next_index];
155 let position = Vec3A::new(
156 lerp(
157 self.positions[prev_index].x,
158 self.positions[next_index].x,
159 interpolation.x.evaluate(frame_t),
160 ),
161 lerp(
162 self.positions[prev_index].y,
163 self.positions[next_index].y,
164 interpolation.y.evaluate(frame_t),
165 ),
166 lerp(
167 self.positions[prev_index].z,
168 self.positions[next_index].z,
169 interpolation.z.evaluate(frame_t),
170 ),
171 );
172
173 let rotation_t = self.rotation_interpolations[next_index].evaluate(frame_t);
174 let rotation =
175 self.rotations[prev_index].slerp(self.rotations[next_index], rotation_t);
176
177 Some((position, rotation))
178 }
179 }
180 }
181
182 fn find_next_keyframe(&self, frame: f32) -> usize {
183 self.frame_numbers
184 .partition_point(|keyframe| (*keyframe as f32) <= frame)
185 }
186
187 fn frame_range(&self) -> Option<(u32, u32)> {
188 Some((*self.frame_numbers.first()?, *self.frame_numbers.last()?))
189 }
190}
191
192#[derive(Clone, Debug)]
193pub struct BoneAnimationBinding {
194 pub bone: BoneIndex,
195 pub track: MovableBoneTrack,
196}
197
198#[derive(Clone, Copy, Debug, PartialEq)]
199pub struct MorphKeyframe {
200 pub frame: u32,
201 pub weight: f32,
202}
203
204impl MorphKeyframe {
205 pub fn new(frame: u32, weight: f32) -> Self {
206 Self { frame, weight }
207 }
208}
209
210#[derive(Clone, Debug)]
211pub struct MorphTrack {
212 frame_numbers: Box<[u32]>,
213 weights: Box<[f32]>,
214}
215
216impl MorphTrack {
217 pub fn from_keyframes(mut keyframes: Vec<MorphKeyframe>) -> Self {
218 keyframes.sort_by_key(|keyframe| keyframe.frame);
219 let mut frame_numbers = Vec::with_capacity(keyframes.len());
220 let mut weights = Vec::with_capacity(keyframes.len());
221 for keyframe in keyframes {
222 frame_numbers.push(keyframe.frame);
223 weights.push(keyframe.weight);
224 }
225 Self {
226 frame_numbers: frame_numbers.into_boxed_slice(),
227 weights: weights.into_boxed_slice(),
228 }
229 }
230
231 pub fn sample(&self, frame: f32) -> Option<f32> {
232 match self.frame_numbers.len() {
233 0 => None,
234 1 => Some(self.weights[0]),
235 _ => {
236 let next_index = self
237 .frame_numbers
238 .partition_point(|keyframe| (*keyframe as f32) <= frame);
239 if next_index == 0 {
240 return Some(self.weights[0]);
241 }
242 if next_index >= self.frame_numbers.len() {
243 return Some(self.weights[self.weights.len() - 1]);
244 }
245
246 let prev_index = next_index - 1;
247 let prev_frame = self.frame_numbers[prev_index] as f32;
248 let next_frame = self.frame_numbers[next_index] as f32;
249 let frame_t = if next_frame == prev_frame {
250 0.0
251 } else {
252 ((frame - prev_frame) / (next_frame - prev_frame)).clamp(0.0, 1.0)
253 };
254 Some(lerp(
255 self.weights[prev_index],
256 self.weights[next_index],
257 frame_t,
258 ))
259 }
260 }
261 }
262
263 fn frame_range(&self) -> Option<(u32, u32)> {
264 Some((*self.frame_numbers.first()?, *self.frame_numbers.last()?))
265 }
266}
267
268#[derive(Clone, Debug)]
269pub struct MorphAnimationBinding {
270 pub morph: MorphIndex,
271 pub track: MorphTrack,
272}
273
274#[derive(Clone, Debug, PartialEq, Eq)]
275pub struct PropertyKeyframe {
276 pub frame: u32,
277 pub ik_enabled: Box<[u8]>,
278}
279
280impl PropertyKeyframe {
281 pub fn new(frame: u32, ik_enabled: Vec<bool>) -> Self {
282 Self {
283 frame,
284 ik_enabled: ik_enabled
285 .into_iter()
286 .map(u8::from)
287 .collect::<Vec<_>>()
288 .into_boxed_slice(),
289 }
290 }
291}
292
293#[derive(Clone, Debug)]
294pub struct PropertyAnimationBinding {
295 frame_numbers: Box<[u32]>,
296 ik_enabled: Box<[Box<[u8]>]>,
297}
298
299impl PropertyAnimationBinding {
300 pub fn from_keyframes(mut keyframes: Vec<PropertyKeyframe>) -> Self {
301 keyframes.sort_by_key(|keyframe| keyframe.frame);
302
303 let mut frame_numbers = Vec::with_capacity(keyframes.len());
304 let mut ik_enabled = Vec::with_capacity(keyframes.len());
305 for keyframe in keyframes {
306 frame_numbers.push(keyframe.frame);
307 ik_enabled.push(keyframe.ik_enabled);
308 }
309
310 Self {
311 frame_numbers: frame_numbers.into_boxed_slice(),
312 ik_enabled: ik_enabled.into_boxed_slice(),
313 }
314 }
315
316 pub fn sample(&self, frame: f32) -> Option<&[u8]> {
317 match self.frame_numbers.len() {
318 0 => None,
319 _ => {
320 let next_index = self
321 .frame_numbers
322 .partition_point(|keyframe| (*keyframe as f32) <= frame);
323 if next_index == 0 {
324 None
325 } else {
326 Some(&self.ik_enabled[next_index - 1])
327 }
328 }
329 }
330 }
331
332 fn frame_range(&self) -> Option<(u32, u32)> {
333 Some((*self.frame_numbers.first()?, *self.frame_numbers.last()?))
334 }
335}
336
337#[derive(Clone, Debug, Default)]
338pub struct AnimationClip {
339 bone_tracks: Box<[BoneAnimationBinding]>,
340 morph_tracks: Box<[MorphAnimationBinding]>,
341 property_track: Option<PropertyAnimationBinding>,
342}
343
344impl AnimationClip {
345 pub fn new(bone_tracks: Vec<BoneAnimationBinding>) -> Self {
346 Self::new_with_morphs(bone_tracks, Vec::new())
347 }
348
349 pub fn new_with_morphs(
350 bone_tracks: Vec<BoneAnimationBinding>,
351 morph_tracks: Vec<MorphAnimationBinding>,
352 ) -> Self {
353 Self::new_full(bone_tracks, morph_tracks, None)
354 }
355
356 pub fn new_full(
357 bone_tracks: Vec<BoneAnimationBinding>,
358 morph_tracks: Vec<MorphAnimationBinding>,
359 property_track: Option<PropertyAnimationBinding>,
360 ) -> Self {
361 Self {
362 bone_tracks: bone_tracks.into_boxed_slice(),
363 morph_tracks: morph_tracks.into_boxed_slice(),
364 property_track,
365 }
366 }
367
368 pub fn apply_to_pose(&self, frame: f32, pose: &mut PoseArena) {
369 pose.reset_local_pose();
370 for binding in self.bone_tracks.iter() {
371 if let Some((position, rotation)) = binding.track.sample(frame) {
372 pose.set_local_position_offset(binding.bone, position);
373 pose.set_local_rotation(binding.bone, rotation);
374 }
375 }
376 for binding in self.morph_tracks.iter() {
377 if let Some(weight) = binding.track.sample(frame) {
378 pose.set_morph_weight(binding.morph, weight);
379 }
380 }
381 if let Some(ik_enabled) = self
382 .property_track
383 .as_ref()
384 .and_then(|track| track.sample(frame))
385 {
386 for (ik_index, enabled) in ik_enabled.iter().enumerate() {
387 pose.set_ik_enabled(ik_index, *enabled != 0);
388 }
389 }
390 }
391
392 pub fn bone_track_count(&self) -> usize {
393 self.bone_tracks.len()
394 }
395
396 pub fn morph_track_count(&self) -> usize {
397 self.morph_tracks.len()
398 }
399
400 pub fn has_property_track(&self) -> bool {
401 self.property_track.is_some()
402 }
403
404 pub fn frame_range(&self) -> Option<(u32, u32)> {
405 let mut range: Option<(u32, u32)> = None;
406 for binding in self.bone_tracks.iter() {
407 merge_frame_range(&mut range, binding.track.frame_range());
408 }
409 for binding in self.morph_tracks.iter() {
410 merge_frame_range(&mut range, binding.track.frame_range());
411 }
412 if let Some(property_track) = self.property_track.as_ref() {
413 merge_frame_range(&mut range, property_track.frame_range());
414 }
415 range
416 }
417
418 pub fn find_bone_track(&self, bone: BoneIndex) -> Option<&MovableBoneTrack> {
419 self.bone_tracks
420 .iter()
421 .find(|binding| binding.bone == bone)
422 .map(|binding| &binding.track)
423 }
424}
425
426fn merge_frame_range(target: &mut Option<(u32, u32)>, range: Option<(u32, u32)>) {
427 let Some((first, last)) = range else {
428 return;
429 };
430 *target = Some(match *target {
431 Some((current_first, current_last)) => (current_first.min(first), current_last.max(last)),
432 None => (first, last),
433 });
434}
435
436fn lerp(a: f32, b: f32, t: f32) -> f32 {
437 a + (b - a) * t
438}
439
440fn bezier_interpolation(x1: f32, x2: f32, y1: f32, y2: f32, x: f32) -> f32 {
441 let mut c = 0.5;
442 let mut t = c;
443 let mut s = 1.0 - t;
444
445 let mut sst3;
446 let mut stt3;
447 let mut ttt;
448
449 for _ in 0..BEZIER_ITERATIONS {
450 sst3 = 3.0 * s * s * t;
451 stt3 = 3.0 * s * t * t;
452 ttt = t * t * t;
453
454 let ft = sst3 * x1 + stt3 * x2 + ttt - x;
455 if ft.abs() < BEZIER_EPSILON {
456 return sst3 * y1 + stt3 * y2 + ttt;
457 }
458
459 c *= 0.5;
460 t += if ft < 0.0 { c } else { -c };
461 s = 1.0 - t;
462 }
463
464 sst3 = 3.0 * s * s * t;
465 stt3 = 3.0 * s * t * t;
466 ttt = t * t * t;
467 sst3 * y1 + stt3 * y2 + ttt
468}
469
470#[cfg(test)]
471mod tests {
472 use glam::{Quat, Vec3A};
473
474 use super::*;
475
476 fn assert_near(actual: f32, expected: f32) {
477 let delta = (actual - expected).abs();
478 assert!(
479 delta < 1.0e-4,
480 "actual={actual:?} expected={expected:?} delta={delta:?}"
481 );
482 }
483
484 fn assert_vec3a_near(actual: Vec3A, expected: Vec3A) {
485 let delta = (actual - expected).abs();
486 assert!(
487 delta.x < 1.0e-4 && delta.y < 1.0e-4 && delta.z < 1.0e-4,
488 "actual={actual:?} expected={expected:?} delta={delta:?}"
489 );
490 }
491
492 #[test]
493 fn linear_interpolation_maps_half_to_half() {
494 assert_near(InterpolationScalar::linear().evaluate(0.5), 0.5);
495 }
496
497 #[test]
498 fn samples_movable_bone_track() {
499 let track = MovableBoneTrack::from_keyframes(vec![
500 MovableBoneKeyframe::new(20, Vec3A::new(10.0, 0.0, 0.0), Quat::IDENTITY),
501 MovableBoneKeyframe::new(10, Vec3A::ZERO, Quat::IDENTITY),
502 ]);
503
504 let (position, rotation) = track.sample(15.0).unwrap();
505
506 assert_vec3a_near(position, Vec3A::new(5.0, 0.0, 0.0));
507 assert_near(rotation.dot(Quat::IDENTITY).abs(), 1.0);
508 }
509
510 #[test]
511 fn samples_morph_track() {
512 let track = MorphTrack::from_keyframes(vec![
513 MorphKeyframe::new(60, 1.0),
514 MorphKeyframe::new(0, 0.0),
515 ]);
516
517 assert_near(track.sample(30.0).unwrap(), 0.5);
518 }
519
520 #[test]
521 fn samples_property_track_as_step_state() {
522 let track = PropertyAnimationBinding::from_keyframes(vec![
523 PropertyKeyframe::new(30, vec![false, true]),
524 PropertyKeyframe::new(0, vec![true, true]),
525 ]);
526
527 assert_eq!(track.sample(-1.0), None);
528 assert_eq!(track.sample(29.0).unwrap(), &[1, 1]);
529 assert_eq!(track.sample(30.0).unwrap(), &[0, 1]);
530 assert_eq!(track.sample(60.0).unwrap(), &[0, 1]);
531 }
532
533 #[test]
534 fn property_track_returns_none_before_first_keyframe() {
535 let track =
536 PropertyAnimationBinding::from_keyframes(vec![PropertyKeyframe::new(30, vec![false])]);
537
538 assert_eq!(track.sample(29.0), None);
539 assert_eq!(track.sample(30.0).unwrap(), &[0]);
540 }
541
542 #[test]
543 fn clip_frame_range_spans_all_track_types() {
544 let bone_track = BoneAnimationBinding {
545 bone: BoneIndex(0),
546 track: MovableBoneTrack::from_keyframes(vec![
547 MovableBoneKeyframe::new(30, Vec3A::ZERO, Quat::IDENTITY),
548 MovableBoneKeyframe::new(10, Vec3A::ZERO, Quat::IDENTITY),
549 ]),
550 };
551 let morph_track = MorphAnimationBinding {
552 morph: MorphIndex(0),
553 track: MorphTrack::from_keyframes(vec![
554 MorphKeyframe::new(20, 0.0),
555 MorphKeyframe::new(60, 1.0),
556 ]),
557 };
558 let property_track = PropertyAnimationBinding::from_keyframes(vec![
559 PropertyKeyframe::new(5, vec![true]),
560 PropertyKeyframe::new(40, vec![false]),
561 ]);
562 let clip =
563 AnimationClip::new_full(vec![bone_track], vec![morph_track], Some(property_track));
564
565 assert_eq!(clip.frame_range(), Some((5, 60)));
566 }
567
568 #[test]
569 fn empty_clip_frame_range_is_none() {
570 assert_eq!(AnimationClip::default().frame_range(), None);
571 }
572}