ranim_core/utils/
bezier.rs

1use approx::{RelativeEq, relative_eq};
2use glam::{DVec3, Vec3Swizzles};
3use itertools::Itertools;
4
5use crate::{
6    prelude::Interpolatable,
7    utils::math::{cross2d, intersection},
8};
9
10/// A path builder based on quadratic beziers
11#[derive(Default)]
12pub struct PathBuilder {
13    start_point: Option<DVec3>,
14    points: Vec<DVec3>,
15}
16
17impl PathBuilder {
18    /// Is the path empty or not
19    pub fn is_empty(&self) -> bool {
20        self.points.is_empty()
21    }
22    /// Construct a new path
23    pub fn new() -> Self {
24        Self::default()
25    }
26    /// Get the number of points
27    pub fn len(&self) -> usize {
28        self.points.len()
29    }
30
31    /// Starts a new subpath and push the point as the start_point
32    pub fn move_to(&mut self, point: DVec3) -> &mut Self {
33        self.start_point = Some(point);
34        if let Some(end) = self.points.last() {
35            self.points.extend_from_slice(&[*end, point]);
36        } else {
37            self.points.push(point);
38        }
39        self
40    }
41
42    fn assert_started(&self) {
43        assert!(
44            self.start_point.is_some() || self.points.is_empty(),
45            "A path have to start with move_to"
46        );
47    }
48
49    /// Append a line
50    pub fn line_to(&mut self, p: DVec3) -> &mut Self {
51        self.assert_started();
52        let mid = (self.points.last().unwrap() + p) / 2.0;
53        self.points.extend_from_slice(&[mid, p]);
54        self
55    }
56
57    /// Append a quadratic bezier
58    pub fn quad_to(&mut self, h: DVec3, p: DVec3) -> &mut Self {
59        self.assert_started();
60        let cur = self.points.last().unwrap();
61        if cur.distance_squared(h) < f64::EPSILON || h.distance_squared(p) < f64::EPSILON {
62            return self.line_to(p);
63        }
64        self.points.extend_from_slice(&[h, p]);
65        self
66    }
67
68    /// Append a cubic bezier
69    pub fn cubic_to(&mut self, h1: DVec3, h2: DVec3, p: DVec3) -> &mut Self {
70        self.assert_started();
71        let cur = self.points.last().unwrap();
72        if cur.distance_squared(h1) < f64::EPSILON || h1.distance_squared(h2) < f64::EPSILON {
73            return self.quad_to(h2, p);
74        }
75        if h2.distance_squared(p) < f64::EPSILON {
76            return self.quad_to(h1, p);
77        }
78
79        let quads = approx_cubic_with_quadratic([*cur, h1, h2, p]);
80        for quad in quads {
81            self.quad_to(quad[1], quad[2]);
82        }
83
84        self
85    }
86
87    /// Close the path.
88    ///
89    /// The path is considered closed if the start point is equal to the last point.
90    ///
91    /// If the path is not closed, a line is appended to the end to close it.
92    pub fn close_path(&mut self) -> &mut Self {
93        self.assert_started();
94        if self.points.last() == self.start_point.as_ref() {
95            return self;
96        }
97        self.line_to(self.start_point.unwrap());
98        self
99    }
100    /// Get the points
101    pub fn vpoints(&self) -> &[DVec3] {
102        &self.points
103    }
104}
105
106/// Split a cubic bezier at progress value `t` in [0.0, 1.0]
107pub fn split_cubic_bezier(bezier: &[DVec3; 4], t: f64) -> ([DVec3; 4], [DVec3; 4]) {
108    let [p0, h0, h1, p1] = bezier;
109
110    let split_point = &cubic_bezier_eval(bezier, t);
111
112    let h00 = p0.lerp(h0, t);
113    let h01 = p0.lerp(h0, t).lerp(h0.lerp(h1, t), t);
114    let h10 = h0.lerp(h1, t).lerp(h1.lerp(p1, t), t);
115    let h11 = h1.lerp(p1, t);
116
117    ([*p0, h00, h01, *split_point], [*split_point, h10, h11, *p1])
118}
119
120/// Split a quad bezier at progress value `t` in [0.0, 1.0]
121pub fn split_quad_bezier(bezier: &[DVec3; 3], t: f64) -> ([DVec3; 3], [DVec3; 3]) {
122    let [p0, h, p1] = bezier;
123
124    let split_point = &quad_bezier_eval(bezier, t);
125
126    let h0 = p0.lerp(h, t);
127    let h1 = h.lerp(p1, t);
128
129    ([*p0, h0, *split_point], [*split_point, h1, *p1])
130}
131
132/// Get [a, b] of a quad bezier
133pub fn trim_quad_bezier(bezier: &[DVec3; 3], a: f64, b: f64) -> [DVec3; 3] {
134    // trace!("!!!trim_quad_bezier: {:?}, {:?}, {:?}", bezier, a, b);
135    let (a, b) = if a > b { (b, a) } else { (a, b) };
136    let end_on_b = split_quad_bezier(bezier, b).0;
137    let a = a / b;
138    split_quad_bezier(&end_on_b, a).1
139}
140
141/// Get [a, b] of a cubic bezier
142pub fn trim_cubic_bezier(bezier: &[DVec3; 4], a: f64, b: f64) -> [DVec3; 4] {
143    // trace!("trim_cubic_bezier: {:?}, {:?}, {:?}", bezier, a, b);
144    let (a, b) = if a > b { (b, a) } else { (a, b) };
145    let end_on_b = split_cubic_bezier(bezier, b).0;
146    // trace!("end_on_b: {:?}", end_on_b);
147    let a = a / b;
148    split_cubic_bezier(&end_on_b, a).1
149}
150
151/// When path is empty, returns None
152pub fn get_subpath_closed_flag<T: RelativeEq>(path: &[T]) -> Option<(usize, bool)> {
153    if path.len() < 3 {
154        return None;
155    }
156    for mut chunk in &path.iter().enumerate().skip(2).chunks(2) {
157        let (a, b) = (chunk.next(), chunk.next());
158        // println!("{:?} {:?}", a, b);
159        if let Some((ia, a)) = match (a, b) {
160            (Some((ia, a)), Some((_ib, b))) => {
161                // println!("chunk[{ia}, {_ib}] {:?}", [a, b]);
162                if a == b { Some((ia, a)) } else { None }
163            }
164            (Some((ia, a)), None) => Some((ia, a)),
165            _ => unreachable!(),
166        } {
167            // println!("### path end ###");
168            if relative_eq!(a, &path[0]) {
169                return Some((ia, true));
170            } else {
171                return Some((ia, false));
172            }
173        }
174    }
175    unreachable!()
176}
177
178/// Returns the point on a quadratic bezier curve at the given parameter.
179pub fn point_on_quadratic_bezier<T: Interpolatable>(points: &[T; 3], t: f64) -> T {
180    let t = t.clamp(0.0, 1.0);
181    let p1 = points[0].lerp(&points[1], t);
182    let p2 = points[1].lerp(&points[2], t);
183    p1.lerp(&p2, t)
184}
185
186/// Returns the control points of the given part of a quadratic bezier curve.
187pub fn partial_quadratic_bezier<T: Interpolatable>(points: &[T; 3], a: f64, b: f64) -> [T; 3] {
188    let a = a.clamp(0.0, 1.0);
189    let b = b.clamp(0.0, 1.0);
190
191    let h0 = point_on_quadratic_bezier(points, a);
192    let h2 = point_on_quadratic_bezier(points, b);
193
194    let h1_prime = points[1].lerp(&points[2], a);
195    let end_prop = (b - a) / (1.0 - a);
196    let h1 = h0.lerp(&h1_prime, end_prop);
197    [h0, h1, h2]
198}
199
200/// Evaluate a cubic bezier at `t` in [0.0, 1.0]
201///
202/// `t` will be clamped into [0.0, 1.0]
203pub fn cubic_bezier_eval(bezier: &[DVec3; 4], t: f64) -> DVec3 {
204    let t = t.clamp(0.0, 1.0);
205    let p0 = bezier[0].lerp(bezier[1], t);
206    let p1 = bezier[1].lerp(bezier[2], t);
207    let p2 = bezier[2].lerp(bezier[3], t);
208
209    let p0 = p0.lerp(p1, t);
210    let p1 = p1.lerp(p2, t);
211
212    p0.lerp(p1, t)
213}
214
215/// Evaluate a quad bezier at `t` in [0.0, 1.0]
216///
217/// `t` will be clamped into [0.0, 1.0]
218pub fn quad_bezier_eval(bezier: &[DVec3; 3], t: f64) -> DVec3 {
219    let t = t.clamp(0.0, 1.0);
220    let p0 = bezier[0].lerp(bezier[1], t);
221    let p1 = bezier[1].lerp(bezier[2], t);
222
223    p0.lerp(p1, t)
224}
225
226/// Approx a cubic bezier with quadratic bezier
227///
228/// [Vec3; 4] is [p1, h1, h2, p2]
229pub fn approx_cubic_with_quadratic(cubic: [DVec3; 4]) -> Vec<[DVec3; 3]> {
230    // trace!("approx cubic {:?}", cubic);
231    let [p1, h1, h2, p2] = cubic;
232
233    let p = h1 - p1;
234    let q = h2 - 2. * h1 + p1;
235    let r = p2 - 3. * h2 + 3. * h1 - p1;
236
237    let a = cross2d(q.xy(), r.xy());
238    let b = cross2d(p.xy(), r.xy());
239    let c = cross2d(p.xy(), q.xy());
240    let disc = b * b - 4. * a * c;
241    let sqrt_disc = disc.sqrt();
242
243    // println!("{} {} {}, disc: {}", a, b, c, disc);
244    let mut root = if a == 0.0 && b == 0.0 || a != 0.0 && disc < 0.0 {
245        0.5
246    } else if a == 0.0 {
247        (-c / b).clamp(0.0, 1.0)
248    } else {
249        let mut root = (-b + sqrt_disc) / (2. * a);
250        if root <= 0.0 || root >= 1.0 {
251            root = (b + sqrt_disc) / (-2. * a);
252        }
253        if root <= 0.0 || root >= 1.0 {
254            root = 0.5
255        }
256        root
257    };
258    if root == 0.0 || root == 1.0 {
259        root = 0.5;
260    }
261    // println!("{root}");
262
263    let cut_point = cubic_bezier_eval(&cubic, root);
264    let cut_tangent = quad_bezier_eval(&[h1 - p1, h2 - h1, p2 - h2], root);
265    let p1_tangent = h1 - p1;
266    let p2_tangent = p2 - h2;
267
268    let i1 = intersection(p1, p1_tangent, cut_point, cut_tangent);
269    let i2 = intersection(p2, p2_tangent, cut_point, cut_tangent);
270    // TODO: Make this better
271    // There is a possibility that cut_tangent equals to p1_tangent or p2_tangent
272    // Example:
273    // ```rust
274    // PathBuilder::new()
275    //     .move_to(vec3(0.0, 2.0, 0.0))
276    //     .cubic_to(
277    //         vec3(-2.0, 2.0, 0.0),
278    //         vec3(1.0, 4.0, 0.0),
279    //         vec3(0.0, 0.0, 0.0),
280    //     )
281    // ```
282    let (Some(i1), Some(i2)) = (i1, i2) else {
283        let root = if root > 0.5 {
284            root / 2.0
285        } else {
286            0.5 + (1.0 - root) / 2.0
287        };
288        let cut_point = cubic_bezier_eval(&cubic, root);
289        let cut_tangent = quad_bezier_eval(&[h1 - p1, h2 - h1, p2 - h2], root);
290        let p1_tangent = h1 - p1;
291        let p2_tangent = p2 - h2;
292        let i1 =
293            intersection(p1, p1_tangent, cut_point, cut_tangent).unwrap_or((p1 + cut_point) / 2.0);
294        let i2 =
295            intersection(p2, p2_tangent, cut_point, cut_tangent).unwrap_or((cut_point + p2) / 2.0);
296        // if i1.is_none() || i2.is_none() {
297        //     panic!(
298        //         r"Can't find intersection for{:?}:
299        //     p1({:?}), p1_tangent({:?}),
300        //     cut_point({:?}), cut_tangent({:?}),
301        //     p2({:?}), p2_tangent({:?})
302        //     ",
303        //         cubic, p1, p1_tangent, cut_point, cut_tangent, p2, p2_tangent
304        //     );
305        // }
306        return vec![[p1, i1, cut_point], [cut_point, i2, p2]];
307    };
308
309    vec![[p1, i1, cut_point], [cut_point, i2, p2]]
310}
311
312#[cfg(test)]
313mod test {
314    use super::*;
315    use glam::dvec3;
316
317    #[test]
318    fn test_trim_quad_bezier() {
319        // 测试正常参数顺序 (a < b)
320        let bezier = [
321            dvec3(0.0, 0.0, 0.0),
322            dvec3(1.0, 2.0, 0.0),
323            dvec3(2.0, 0.0, 0.0),
324        ];
325        let trimmed = trim_quad_bezier(&bezier, 0.25, 0.75);
326
327        // 验证结果点在曲线上
328        let start_point = quad_bezier_eval(&bezier, 0.25);
329        let end_point = quad_bezier_eval(&bezier, 0.75);
330        assert!((trimmed[0] - start_point).length() < f64::EPSILON);
331        assert!((trimmed[2] - end_point).length() < f64::EPSILON);
332
333        // 测试参数顺序颠倒 (a > b)
334        let trimmed_reversed = trim_quad_bezier(&bezier, 0.75, 0.25);
335        assert!((trimmed_reversed[0] - start_point).length() < f64::EPSILON);
336        assert!((trimmed_reversed[2] - end_point).length() < f64::EPSILON);
337
338        // 测试边界值
339        let full_curve = trim_quad_bezier(&bezier, 0.0, 1.0);
340        assert!((full_curve[0] - bezier[0]).length() < f64::EPSILON);
341        assert!((full_curve[2] - bezier[2]).length() < f64::EPSILON);
342
343        // 测试零长度曲线
344        let zero_length = trim_quad_bezier(&bezier, 0.5, 0.5);
345        let mid_point = quad_bezier_eval(&bezier, 0.5);
346        assert!((zero_length[0] - mid_point).length() < f64::EPSILON);
347        assert!((zero_length[2] - mid_point).length() < f64::EPSILON);
348
349        // 测试复杂曲线
350        let complex_bezier = [
351            dvec3(-1.0, 0.0, 1.0),
352            dvec3(1.0, 3.0, 0.0),
353            dvec3(3.0, 0.0, -1.0),
354        ];
355        let trimmed_complex = trim_quad_bezier(&complex_bezier, 0.2, 0.8);
356        let complex_start = quad_bezier_eval(&complex_bezier, 0.2);
357        let complex_end = quad_bezier_eval(&complex_bezier, 0.8);
358        assert!((trimmed_complex[0] - complex_start).length() < f64::EPSILON);
359        assert!((trimmed_complex[2] - complex_end).length() < f64::EPSILON);
360    }
361}
ranim_core/utils/bezier.rs

ranim_core/utils/
bezier.rs
