oxygengine_animation/
curve.rs

1use crate::range_iter;
2use core::Scalar;
3use serde::{Deserialize, Serialize};
4use std::sync::{Arc, RwLock};
5
6const EPSILON: Scalar = Scalar::EPSILON * 10.0;
7
8pub trait Curved {
9    fn zero() -> Self;
10    fn one() -> Self;
11    fn negate(&self) -> Self;
12    fn scale(&self, value: Scalar) -> Self;
13    fn inverse_scale(&self, value: Scalar) -> Self;
14    fn length(&self) -> Scalar;
15    fn length_squared(&self) -> Scalar;
16    fn get_axis(&self, index: usize) -> Option<Scalar>;
17    fn interpolate(&self, other: &Self, factor: Scalar) -> Self;
18    fn is_valid(&self) -> bool;
19}
20
21impl Curved for Scalar {
22    fn zero() -> Self {
23        0.0
24    }
25
26    fn one() -> Self {
27        1.0
28    }
29
30    fn negate(&self) -> Self {
31        -self
32    }
33
34    fn scale(&self, value: Scalar) -> Self {
35        self * value
36    }
37
38    fn inverse_scale(&self, value: Scalar) -> Self {
39        self / value
40    }
41
42    fn length(&self) -> Scalar {
43        self.abs()
44    }
45
46    fn length_squared(&self) -> Scalar {
47        self * self
48    }
49
50    fn get_axis(&self, index: usize) -> Option<Scalar> {
51        match index {
52            0 => Some(*self),
53            _ => None,
54        }
55    }
56
57    fn interpolate(&self, other: &Self, factor: Scalar) -> Self {
58        let diff = other - self;
59        diff * factor + self
60    }
61
62    fn is_valid(&self) -> bool {
63        self.is_finite()
64    }
65}
66
67impl Curved for (Scalar, Scalar) {
68    fn zero() -> Self {
69        (0.0, 0.0)
70    }
71
72    fn one() -> Self {
73        (1.0, 1.0)
74    }
75
76    fn negate(&self) -> Self {
77        (-self.0, -self.1)
78    }
79
80    fn scale(&self, value: Scalar) -> Self {
81        (self.0 * value, self.1 * value)
82    }
83
84    fn inverse_scale(&self, value: Scalar) -> Self {
85        (self.0 / value, self.1 / value)
86    }
87
88    fn length(&self) -> Scalar {
89        self.length_squared().sqrt()
90    }
91
92    fn length_squared(&self) -> Scalar {
93        self.0 * self.0 + self.1 * self.1
94    }
95
96    fn get_axis(&self, index: usize) -> Option<Scalar> {
97        match index {
98            0 => Some(self.0),
99            1 => Some(self.1),
100            _ => None,
101        }
102    }
103
104    fn interpolate(&self, other: &Self, factor: Scalar) -> Self {
105        let diff0 = other.0 - self.0;
106        let diff1 = other.1 - self.1;
107        (diff0 * factor + self.0, diff1 * factor + self.1)
108    }
109
110    fn is_valid(&self) -> bool {
111        self.0.is_valid() && self.1.is_valid()
112    }
113}
114
115impl<T> Curved for Arc<RwLock<T>>
116where
117    T: Curved,
118{
119    fn zero() -> Self {
120        Arc::new(RwLock::new(T::zero()))
121    }
122
123    fn one() -> Self {
124        Arc::new(RwLock::new(T::one()))
125    }
126
127    fn negate(&self) -> Self {
128        Arc::new(RwLock::new(self.read().unwrap().negate()))
129    }
130
131    fn scale(&self, value: Scalar) -> Self {
132        Arc::new(RwLock::new(self.read().unwrap().scale(value)))
133    }
134
135    fn inverse_scale(&self, value: Scalar) -> Self {
136        Arc::new(RwLock::new(self.read().unwrap().inverse_scale(value)))
137    }
138
139    fn length(&self) -> Scalar {
140        self.read().unwrap().length()
141    }
142
143    fn length_squared(&self) -> Scalar {
144        self.read().unwrap().length_squared()
145    }
146
147    fn get_axis(&self, index: usize) -> Option<Scalar> {
148        self.read().unwrap().get_axis(index)
149    }
150
151    fn interpolate(&self, other: &Self, factor: Scalar) -> Self {
152        let from: &T = &self.read().unwrap();
153        let to: &T = &other.read().unwrap();
154        let value = from.interpolate(to, factor);
155        Arc::new(RwLock::new(value))
156    }
157
158    fn is_valid(&self) -> bool {
159        self.read().unwrap().is_valid()
160    }
161}
162
163pub trait CurvedChange {
164    fn offset(&self, other: &Self) -> Self;
165    fn delta(&self, other: &Self) -> Self;
166    fn dot(&self, other: &Self) -> Scalar;
167}
168
169impl CurvedChange for (Scalar, Scalar) {
170    fn offset(&self, other: &Self) -> Self {
171        (self.0 + other.0, self.1 + other.1)
172    }
173
174    fn delta(&self, other: &Self) -> Self {
175        (other.0 - self.0, other.1 - self.1)
176    }
177
178    fn dot(&self, other: &Self) -> Scalar {
179        self.0 * other.0 + self.1 * other.1
180    }
181}
182
183impl<T> CurvedChange for Arc<RwLock<T>>
184where
185    T: CurvedChange,
186{
187    fn offset(&self, other: &Self) -> Self {
188        let from: &T = &self.read().unwrap();
189        let to: &T = &other.read().unwrap();
190        Arc::new(RwLock::new(from.offset(to)))
191    }
192
193    fn delta(&self, other: &Self) -> Self {
194        let from: &T = &self.read().unwrap();
195        let to: &T = &other.read().unwrap();
196        Arc::new(RwLock::new(from.delta(to)))
197    }
198
199    fn dot(&self, other: &Self) -> Scalar {
200        let from: &T = &self.read().unwrap();
201        let to: &T = &other.read().unwrap();
202        from.dot(to)
203    }
204}
205
206#[derive(Debug, Clone, Serialize, Deserialize)]
207pub struct CurveDef<T>(pub T, pub T, pub T, pub T);
208
209impl<T> Default for CurveDef<T>
210where
211    T: Curved,
212{
213    fn default() -> Self {
214        Self(T::zero(), T::zero(), T::one(), T::one())
215    }
216}
217
218impl<T> TryFrom<CurveDef<T>> for Curve<T>
219where
220    T: Clone + Curved + CurvedChange,
221{
222    type Error = CurveError;
223
224    fn try_from(value: CurveDef<T>) -> Result<Self, Self::Error> {
225        Self::bezier(value.0, value.1, value.2, value.3)
226    }
227}
228
229impl<T> From<Curve<T>> for CurveDef<T>
230where
231    T: Clone + Curved + CurvedChange,
232{
233    fn from(v: Curve<T>) -> Self {
234        Self(v.from, v.from_param, v.to_param, v.to)
235    }
236}
237
238#[derive(Debug, Copy, Clone, Serialize, Deserialize)]
239pub enum CurveError {
240    InvalidFromValue,
241    InvalidFromParamValue,
242    InvalidToParamValue,
243    InvalidToValue,
244    CannotSplit,
245}
246
247impl std::fmt::Display for CurveError {
248    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
249        write!(f, "{:?}", self)
250    }
251}
252
253#[derive(Debug, Clone, Serialize, Deserialize)]
254#[serde(try_from = "CurveDef<T>")]
255#[serde(into = "CurveDef<T>")]
256pub struct Curve<T>
257where
258    T: Clone + Curved + CurvedChange,
259{
260    from: T,
261    from_param: T,
262    to_param: T,
263    to: T,
264    length: Scalar,
265}
266
267impl<T> Default for Curve<T>
268where
269    T: Clone + Curved + CurvedChange,
270{
271    fn default() -> Self {
272        Self::linear(T::zero(), T::one()).unwrap()
273    }
274}
275
276impl<T> Curve<T>
277where
278    T: Clone + Curved + CurvedChange,
279{
280    fn new_uninitialized(from: T, from_param: T, to_param: T, to: T) -> Result<Self, CurveError> {
281        if !from.is_valid() {
282            return Err(CurveError::InvalidFromValue);
283        }
284        if !from_param.is_valid() {
285            return Err(CurveError::InvalidFromParamValue);
286        }
287        if !to_param.is_valid() {
288            return Err(CurveError::InvalidToParamValue);
289        }
290        if !to.is_valid() {
291            return Err(CurveError::InvalidToValue);
292        }
293        Ok(Self {
294            from,
295            from_param,
296            to_param,
297            to,
298            length: 0.0,
299        })
300    }
301
302    pub fn linear(from: T, to: T) -> Result<Self, CurveError> {
303        let mut result = Self::new_uninitialized(from.clone(), to.clone(), from, to)?;
304        result.recalculate_length();
305        Ok(result)
306    }
307
308    pub fn bezier(from: T, from_param: T, to_param: T, to: T) -> Result<Self, CurveError> {
309        let mut result = Self::new_uninitialized(from, from_param, to_param, to)?;
310        result.recalculate_length();
311        Ok(result)
312    }
313
314    fn recalculate_length(&mut self) {
315        self.length = self.arc_length(EPSILON);
316    }
317
318    pub fn from(&self) -> &T {
319        &self.from
320    }
321
322    pub fn set_from(&mut self, value: T) {
323        self.from = value;
324        self.recalculate_length();
325    }
326
327    pub fn from_param(&self) -> &T {
328        &self.from_param
329    }
330
331    pub fn set_from_param(&mut self, value: T) {
332        self.from_param = value;
333        self.recalculate_length();
334    }
335
336    pub fn to_param(&self) -> &T {
337        &self.to_param
338    }
339
340    pub fn set_to_param(&mut self, value: T) {
341        self.to_param = value;
342        self.recalculate_length();
343    }
344
345    pub fn to(&self) -> &T {
346        &self.to
347    }
348
349    pub fn set_to(&mut self, value: T) {
350        self.to = value;
351        self.recalculate_length();
352    }
353
354    pub fn set(&mut self, from: T, from_param: T, to_param: T, to: T) {
355        self.from = from;
356        self.from_param = from_param;
357        self.to_param = to_param;
358        self.to = to;
359        self.recalculate_length();
360    }
361
362    pub fn length(&self) -> Scalar {
363        self.length
364    }
365
366    pub fn value_along_axis_iter(
367        &self,
368        steps: usize,
369        axis_index: usize,
370    ) -> Option<impl Iterator<Item = Scalar>> {
371        let from = self.from.get_axis(axis_index)?;
372        let to = self.to.get_axis(axis_index)?;
373        Some(range_iter(steps, from, to))
374    }
375
376    #[allow(clippy::many_single_char_names)]
377    pub fn sample(&self, mut factor: Scalar) -> T {
378        factor = factor.max(0.0).min(1.0);
379        let a = self.from.interpolate(&self.from_param, factor);
380        let b = self.from_param.interpolate(&self.to_param, factor);
381        let c = self.to_param.interpolate(&self.to, factor);
382        let d = a.interpolate(&b, factor);
383        let e = b.interpolate(&c, factor);
384        d.interpolate(&e, factor)
385    }
386
387    pub fn sample_along_axis(&self, axis_value: Scalar, axis_index: usize) -> Option<T> {
388        let factor = self.find_time_for_axis(axis_value, axis_index)?;
389        Some(self.sample(factor))
390    }
391
392    /// Velocity of change along the curve.
393    pub fn sample_first_derivative(&self, mut factor: Scalar) -> T {
394        factor = factor.max(0.0).min(1.0);
395        let a = self.from.delta(&self.from_param);
396        let b = self.from_param.delta(&self.to_param);
397        let c = self.to_param.delta(&self.to);
398        let d = a.interpolate(&b, factor);
399        let e = b.interpolate(&c, factor);
400        d.interpolate(&e, factor).scale(3.0)
401    }
402
403    /// Velocity of change along the curve axis.
404    pub fn sample_first_derivative_along_axis(
405        &self,
406        axis_value: Scalar,
407        axis_index: usize,
408    ) -> Option<T> {
409        let factor = self.find_time_for_axis(axis_value, axis_index)?;
410        Some(self.sample_first_derivative(factor))
411    }
412
413    /// Acceleration of change along the curve.
414    pub fn sample_second_derivative(&self, mut factor: Scalar) -> T {
415        factor = factor.max(0.0).min(1.0);
416        let a = self.from.delta(&self.from_param);
417        let b = self.from_param.delta(&self.to_param);
418        let c = self.to_param.delta(&self.to);
419        let d = a.delta(&b);
420        let e = b.delta(&c);
421        d.interpolate(&e, factor).scale(6.0)
422    }
423
424    /// Acceleration of change along the curve axis.
425    pub fn sample_second_derivative_along_axis(
426        &self,
427        axis_value: Scalar,
428        axis_index: usize,
429    ) -> Option<T> {
430        let factor = self.find_time_for_axis(axis_value, axis_index)?;
431        Some(self.sample_second_derivative(factor))
432    }
433
434    pub fn sample_k(&self, mut factor: Scalar) -> Scalar {
435        factor = factor.max(0.0).min(1.0);
436        let first = self.sample_first_derivative(factor);
437        let second = self.sample_second_derivative(factor);
438        second.length() / first.length().powf(1.5)
439    }
440
441    pub fn sample_curvature_radius(&self, factor: Scalar) -> Scalar {
442        1.0 / self.sample_k(factor)
443    }
444
445    pub fn sample_tangent(&self, mut factor: Scalar) -> T {
446        factor = factor.clamp(EPSILON, 1.0 - EPSILON);
447        let direction = self.sample_first_derivative(factor);
448        let length = direction.length();
449        direction.inverse_scale(length)
450    }
451
452    pub fn sample_tangent_along_axis(&self, axis_value: Scalar, axis_index: usize) -> Option<T> {
453        let factor = self.find_time_for_axis(axis_value, axis_index)?;
454        Some(self.sample_tangent(factor))
455    }
456
457    fn split_uninitialized(&self, mut factor: Scalar) -> Result<(Self, Self), CurveError> {
458        factor = factor.max(0.0).min(1.0);
459        #[allow(clippy::manual_range_contains)]
460        if factor < EPSILON || factor > 1.0 - EPSILON {
461            return Err(CurveError::CannotSplit);
462        }
463        let a = self.from.interpolate(&self.from_param, factor);
464        let b = self.from_param.interpolate(&self.to_param, factor);
465        let c = self.to_param.interpolate(&self.to, factor);
466        let d = a.interpolate(&b, factor);
467        let e = b.interpolate(&c, factor);
468        let f = d.interpolate(&e, factor);
469        let first = Self::new_uninitialized(self.from.clone(), a, d, f.clone())?;
470        let second = Self::new_uninitialized(f, e, c, self.to.clone())?;
471        Ok((first, second))
472    }
473
474    pub fn split(&self, factor: Scalar) -> Result<(Self, Self), CurveError> {
475        self.split_uninitialized(factor).map(|(mut a, mut b)| {
476            a.recalculate_length();
477            b.recalculate_length();
478            (a, b)
479        })
480    }
481
482    fn estimate_arc_length(&self) -> Scalar {
483        let a = self.from.delta(&self.from_param).length();
484        let b = self.from_param.delta(&self.to_param).length();
485        let c = self.to_param.delta(&self.to).length();
486        let d = self.to.delta(&self.from).length();
487        (a + b + c + d) * 0.5
488    }
489
490    fn arc_length(&self, threshold: Scalar) -> Scalar {
491        self.arc_length_inner(self.estimate_arc_length(), threshold, 5)
492    }
493
494    fn arc_length_inner(&self, estimation: Scalar, threshold: Scalar, levels: usize) -> Scalar {
495        let (a, b) = match self.split_uninitialized(0.5) {
496            Ok((a, b)) => (a, b),
497            Err(_) => return estimation,
498        };
499        let ra = a.estimate_arc_length();
500        let rb = b.estimate_arc_length();
501        let result = ra + rb;
502        if (estimation - result).abs() < threshold || levels == 0 {
503            return result;
504        }
505        let levels = levels - 1;
506        let a = a.arc_length_inner(ra, threshold, levels);
507        let b = b.arc_length_inner(rb, threshold, levels);
508        a + b
509    }
510
511    pub fn find_time_for_axis(&self, mut axis_value: Scalar, axis_index: usize) -> Option<Scalar> {
512        let min = self.from.get_axis(axis_index)?;
513        let max = self.to.get_axis(axis_index)?;
514        let dist = max - min;
515        if dist.abs() < EPSILON {
516            return Some(1.0);
517        }
518        axis_value = axis_value.max(min).min(max);
519        let mut guess = (axis_value - min) / dist;
520        let mut last_tangent = None;
521        for _ in 0..5 {
522            let dv = self.sample(guess).get_axis(axis_index)? - axis_value;
523            if dv.abs() < EPSILON {
524                return Some(guess);
525            }
526            let tangent = self.sample_tangent(guess);
527            let slope = if let Some(last_tangent) = last_tangent {
528                tangent.dot(&last_tangent)
529            } else {
530                1.0
531            };
532            last_tangent = Some(tangent);
533            guess -= dv * slope;
534        }
535        Some(guess)
536    }
537
538    /// initial time guess, number of iterations, fn(time, curve) -> time
539    pub fn find_time_for(
540        &self,
541        mut guess: Scalar,
542        iterations: usize,
543        mut f: impl FnMut(Scalar, &Self) -> Scalar,
544    ) -> Scalar {
545        guess = guess.clamp(0.0, 1.0);
546        for _ in 0..iterations {
547            let time = f(guess, self);
548            if (time - guess).abs() < EPSILON {
549                return time;
550            }
551            guess = time;
552        }
553        guess
554    }
555}
oxygengine_animation/curve.rs

oxygengine_animation/
curve.rs
