adaptive_bezier/
lib.rs

1use std::f64::consts::PI;
2
3pub type Vector2 = nalgebra::Vector2<f64>;
4
5const TAU: f64 = 2.0 * PI;
6
7const RECUSION_LIMIT: u32 = 8;
8const FLOAT_EPSILON: f64 = 1.19209290e-7;
9const PATH_DISTANCE_EPSILON: f64 = 1.0;
10
11const CURVE_ANGLE_TOERANCE_EPSILON: f64 = 0.01;
12const M_ANGLE_TOLERANCE: f64 = 0.0;
13const M_CUSP_LIMIT: f64 = 0.0;
14
15#[inline]
16fn clamp_angle(x: f64) -> f64 {
17    if x >= PI {
18        TAU - x
19    } else {
20        x
21    }
22}
23
24pub fn adaptive_bezier_curve(
25    start: Vector2,
26    c1: Vector2,
27    c2: Vector2,
28    end: Vector2,
29    scale: f64,
30) -> Vec<Vector2> {
31    let distance_tolerance = (PATH_DISTANCE_EPSILON / scale).powi(2);
32    let mut sample_points = Vec::new();
33    sample_points.push(start);
34    adaptive_bezier_curve_impl(
35        start,
36        c1,
37        c2,
38        end,
39        &mut sample_points,
40        distance_tolerance,
41        0,
42    );
43    sample_points.push(end);
44    sample_points
45}
46
47pub fn adaptive_bezier_curve_impl(
48    p1: Vector2,
49    p2: Vector2,
50    p3: Vector2,
51    p4: Vector2,
52    points: &mut Vec<Vector2>,
53    distance_tolerance: f64,
54    level: u32,
55) {
56    if level > RECUSION_LIMIT {
57        return;
58    }
59    let p12 = (p1 + p2) / 2.0;
60    let p23 = (p2 + p3) / 2.0;
61    let p34 = (p3 + p4) / 2.0;
62    let p123 = (p12 + p23) / 2.0;
63    let p234 = (p23 + p34) / 2.0;
64    let p1234 = (p123 + p234) / 2.0;
65    if level > 0 {
66        // Enforce subdivision first time
67        // Try to approximate the full cubic curve by a single straight line
68        let d = p4 - p1;
69        let d2 = (p2 - p4).perp(&d).abs();
70        let d3 = (p3 - p4).perp(&d).abs();
71        if d2 > FLOAT_EPSILON && d3 > FLOAT_EPSILON {
72            // Regular care
73            if (d2 + d3).powi(2) <= distance_tolerance * d.norm_squared() {
74                // If the curvature doesn't exceed the distanceTolerance value
75                // we tend to finish subdivisions
76                if M_ANGLE_TOLERANCE < CURVE_ANGLE_TOERANCE_EPSILON {
77                    points.push(p1234);
78                    return;
79                }
80                // Angle & cusp condition
81                // TODO: replace here with angle
82                let a23 = (p3.y - p2.y).atan2(p3.x - p2.x);
83                let da1 = clamp_angle((a23 - (p2.y - p1.y).atan2(p2.x - p1.x)).abs());
84                let da2 = clamp_angle(((p4.y - p3.y).atan2(p4.x - p3.x) - a23).abs());
85                if da1 + da2 < M_ANGLE_TOLERANCE {
86                    // Finally we can stop the recursion
87                    points.push(p1234);
88                    return;
89                }
90                if M_CUSP_LIMIT != 0.0 {
91                    if da1 > M_CUSP_LIMIT {
92                        points.push(p2);
93                        return;
94                    }
95                    if da2 > M_CUSP_LIMIT {
96                        points.push(p3);
97                        return;
98                    }
99                }
100            }
101        } else if d2 > FLOAT_EPSILON {
102            // P_1, P_3, P_4 are collinear, P_2 is considerable
103            if d2 * d2 <= distance_tolerance * d.norm_squared() {
104                if M_ANGLE_TOLERANCE < CURVE_ANGLE_TOERANCE_EPSILON {
105                    points.push(p1234);
106                    return;
107                }
108                // Angle condition
109                let da1 = clamp_angle(
110                    ((p3.y - p2.y).atan2(p3.x - p2.x) - (p2.y - p1.y).atan2(p2.x - p1.x)).abs(),
111                );
112                if da1 < M_ANGLE_TOLERANCE {
113                    points.push(p2);
114                    points.push(p3);
115                    return;
116                }
117                if M_CUSP_LIMIT != 0.0 && da1 > M_CUSP_LIMIT {
118                    points.push(p2);
119                    return;
120                }
121            }
122        } else if d3 > FLOAT_EPSILON {
123            // P_1, P_2, P_4 are collinear, P_3 is considerable
124            if d3 * d3 <= distance_tolerance * d.norm_squared() {
125                if M_ANGLE_TOLERANCE < CURVE_ANGLE_TOERANCE_EPSILON {
126                    points.push(p1234);
127                    return;
128                }
129                // Angle condition
130                let da1 = clamp_angle(
131                    ((p4.y - p3.y).atan2(p4.x - p3.x) - (p3.y - p2.y).atan2(p3.x - p2.x)).abs(),
132                );
133                if da1 < M_ANGLE_TOLERANCE {
134                    points.push(p2);
135                    points.push(p3);
136                    return;
137                }
138                if M_CUSP_LIMIT != 0.0 && da1 > M_CUSP_LIMIT {
139                    points.push(p3);
140                    return;
141                }
142            }
143        } else {
144            // Collinear case
145            let d = p1234 - (p1 + p4) / 2.0;
146            if d.norm_squared() < distance_tolerance {
147                points.push(p1234);
148                return;
149            }
150        }
151    }
152    // Continue subdivision
153    adaptive_bezier_curve_impl(p1, p12, p123, p1234, points, distance_tolerance, level + 1);
154    adaptive_bezier_curve_impl(p1234, p234, p34, p4, points, distance_tolerance, level + 1);
155}
156
157#[cfg(test)]
158mod tests {
159    use super::{adaptive_bezier_curve, Vector2, FLOAT_EPSILON};
160
161    #[test]
162    fn simple() {
163        let answer = vec![
164            Vector2::new(20.0, 20.0),
165            Vector2::new(27.12921142578125, 32.740386962890625),
166            Vector2::new(39.34417724609375, 56.408416748046875),
167            Vector2::new(53.46435546875, 87.040771484375),
168            Vector2::new(63.99200439453125, 111.28897094726562),
169            Vector2::new(69.82025146484375, 123.60739135742188),
170            Vector2::new(73.9102554321289, 130.69841384887695),
171            Vector2::new(76.61632537841797, 134.36077499389648),
172            Vector2::new(79.37641143798828, 137.14487075805664),
173            Vector2::new(82.25093841552734, 139.02817916870117),
174            Vector2::new(85.30033111572266, 139.98817825317383),
175            Vector2::new(88.58501434326172, 140.00234603881836),
176            Vector2::new(92.16541290283203, 139.04816055297852),
177            Vector2::new(96.1019515991211, 137.10309982299805),
178            Vector2::new(100.4550552368164, 134.1446418762207),
179            Vector2::new(105.28514862060547, 130.15026473999023),
180            Vector2::new(113.55682373046875, 122.16708374023438),
181            Vector2::new(126.81915283203125, 107.68692016601562),
182            Vector2::new(143.07708740234375, 88.65768432617188),
183            Vector2::new(174.13818359375, 51.190185546875),
184            Vector2::new(200.0, 20.0),
185        ];
186        let start = Vector2::new(20.0, 20.0);
187        let c1 = Vector2::new(100.0, 159.0);
188        let c2 = Vector2::new(50.0, 200.0);
189        let end = Vector2::new(200.0, 20.0);
190        let scale = 2.0;
191        let output = adaptive_bezier_curve(start, c1, c2, end, scale);
192        assert_eq!(output.len(), answer.len());
193        for i in 0..answer.len() {
194            assert_eq!((output[i].x - answer[i].x).abs() < FLOAT_EPSILON, true);
195            assert_eq!((output[i].y - answer[i].y).abs() < FLOAT_EPSILON, true);
196        }
197    }
198}
adaptive_bezier/lib.rs

adaptive_bezier/
lib.rs
