1use std::mem;
6
7use smallvec::SmallVec;
8
9use crate::math;
10
11const TOLERANCE: f32 = 0.005;
12
13fn angle(origin: math::Vec, p0: math::Vec, p1: math::Vec) -> f32 {
14 let d0 = p0 - origin;
15 let d1 = p1 - origin;
16
17 let cross = d0.x * d1.y - d0.y * d1.x;
18 let dot = d0.x * d1.x + d0.y * d1.y;
19
20 cross.atan2(dot)
21}
22
23fn line_segment_intersection(
24 segment0: [math::Vec; 2],
25 segment1: [math::Vec; 2],
26) -> Option<math::Vec> {
27 let d10 = segment0[1] - segment0[0];
28 let d32 = segment1[1] - segment1[0];
29
30 let denom = d10.x * d32.y - d32.x * d10.y;
31 if denom == 0.0 {
32 return None;
33 }
34
35 let denom_is_pos = denom > 0.0;
36
37 let d02 = segment0[0] - segment1[0];
38 let s_numer = d10.x * d02.y - d10.y * d02.x;
39 if (s_numer < 0.0) == denom_is_pos {
40 return None;
41 }
42
43 let t_numer = d32.x * d02.y - d32.y * d02.x;
44 if (t_numer < 0.0) == denom_is_pos {
45 return None;
46 }
47
48 if (s_numer > denom) == denom_is_pos || (t_numer > denom) == denom_is_pos {
49 return None;
50 }
51
52 let t = t_numer / denom;
53 Some(segment0[0] + d10 * t)
54}
55
56fn align(point: math::Vec, line: [math::Vec; 2]) -> math::Vec {
57 let angle = -(line[1].y - line[0].y).atan2(line[1].x - line[0].x);
58
59 math::Vec::new(
60 (point.x - line[0].x) * angle.cos() - (point.y - line[0].y) * angle.sin(),
61 (point.x - line[0].x) * angle.sin() + (point.y - line[0].y) * angle.cos(),
62 )
63}
64
65fn approx_eq(p0: math::Vec, p1: math::Vec) -> bool {
66 (p0.x - p1.x).abs() <= TOLERANCE && (p0.y - p1.y).abs() <= TOLERANCE
67}
68
69fn left_different(points: &[math::Vec]) -> [math::Vec; 2] {
70 points
71 .windows(2)
72 .find_map(|window| {
73 if let [p0, p1] = *window {
74 (!approx_eq(p0, p1)).then_some([p0, p1])
75 } else {
76 unreachable!()
77 }
78 })
79 .expect("Bezier cannot be a point")
80}
81
82pub fn right_different(points: &[math::Vec]) -> [math::Vec; 2] {
83 points
84 .windows(2)
85 .rev()
86 .find_map(|window| {
87 if let [p0, p1] = *window {
88 (!approx_eq(p0, p1)).then_some([p0, p1])
89 } else {
90 unreachable!()
91 }
92 })
93 .expect("Bezier cannot be a point")
94}
95
96fn normal(p0: math::Vec, p1: math::Vec, c: f32) -> Option<math::Vec> {
97 let d = (p1 - p0) * c;
98 if d.x.abs() == 0.0 && d.y.abs() == 0.0 {
99 return None;
100 }
101
102 let q = (d.x * d.x + d.y * d.y).sqrt();
103
104 Some(math::Vec::new(-d.y / q, d.x / q))
105}
106
107fn normal_left(points: &[math::Vec]) -> math::Vec {
108 let c = points.len() as f32 - 1.0;
109 let [p0, p1] = left_different(points);
110
111 normal(p0, p1, c).expect("Bezier cannot be a point")
112}
113
114fn normal_right(points: &[math::Vec]) -> math::Vec {
115 let c = points.len() as f32 - 1.0;
116 let [p0, p1] = right_different(points);
117
118 normal(p0, p1, c).expect("Bezier cannot be a point")
119}
120
121fn derive(points: &[math::Vec]) -> SmallVec<[math::Vec; 3]> {
122 let mut derived = SmallVec::new();
123
124 for window in points.windows(2) {
125 if let [p0, p1] = *window {
126 derived.push((p1 - p0) * (points.len() - 1) as f32);
127 }
128 }
129
130 derived
131}
132
133fn derive_left(points: &[math::Vec]) -> math::Vec {
134 let [p0, p1] = left_different(points);
135 (p1 - p0) * (points.len() - 1) as f32
136}
137
138fn derive_right(points: &[math::Vec]) -> math::Vec {
139 let [p0, p1] = right_different(points);
140 (p1 - p0) * (points.len() - 1) as f32
141}
142
143fn droots(vals: &[f32]) -> SmallVec<[f32; 2]> {
144 let mut droots = SmallVec::new();
145
146 match *vals {
147 [a, b] => {
148 if a != b {
149 droots.push(a / (a - b));
150 }
151 }
152 [a, b, c] => {
153 let d = a - 2.0 * b + c;
154 if d != 0.0 {
155 let m1 = -(b * b - a * c).sqrt();
156 let m2 = -a + b;
157
158 droots.push(-(m1 + m2) / d);
159 droots.push(-(-m1 + m2) / d);
160 } else if b != c && d == 0.0 {
161 droots.push((2.0 * b - c) / (2.0 * (b - c)));
162 }
163 }
164 _ => (),
165 }
166
167 droots
168}
169
170fn hull(points: &[math::Vec; 4], t: f32) -> SmallVec<[math::Vec; 10]> {
171 let mut hull = SmallVec::new();
172 let mut points = SmallVec::from(*points);
173 let mut next_points = SmallVec::new();
174
175 hull.extend(points.iter().copied());
176
177 while points.len() > 1 {
178 next_points.clear();
179
180 for window in points.windows(2) {
181 if let [p0, p1] = *window {
182 let point = p0.lerp(p1, t);
183 hull.push(point);
184 next_points.push(point);
185 }
186 }
187
188 mem::swap(&mut points, &mut next_points);
189 }
190
191 hull
192}
193
194fn split(points: &[math::Vec; 4], t0: f32, t1: f32) -> [math::Vec; 4] {
195 let hull0 = hull(points, t0);
196 let right = [hull0[9], hull0[8], hull0[6], hull0[3]];
197
198 let t1 = (t1 - t0) / (1.0 - t0);
199 let hull1 = hull(&right, t1);
200
201 [hull1[0], hull1[4], hull1[7], hull1[9]]
202}
203
204fn line_intersection(
205 p0: math::Vec,
206 p1: math::Vec,
207 p2: math::Vec,
208 p3: math::Vec,
209) -> Option<math::Vec> {
210 let nx =
211 (p0.x * p1.y - p0.y * p1.x) * (p2.x - p3.x) - (p0.x - p1.x) * (p2.x * p3.y - p2.y * p3.x);
212 let ny =
213 (p0.x * p1.y - p0.y * p1.x) * (p2.y - p3.y) - (p0.y - p1.y) * (p2.x * p3.y - p2.y * p3.x);
214 let d = (p0.x - p1.x) * (p2.y - p3.y) - (p0.y - p1.y) * (p2.x - p3.x);
215
216 if d == 0.0 {
217 return None;
218 }
219
220 Some(math::Vec::new(nx / d, ny / d))
221}
222
223fn scale(points: &[math::Vec; 4], dist: f32) -> Option<[math::Vec; 4]> {
224 if dist == 0.0 {
225 return Some(*points);
226 }
227
228 let normals = [normal_left(points), normal_right(points)];
229 let offset = [points[0] + normals[0] * 10.0, points[3] + normals[1] * 10.0];
230 let origin = line_intersection(offset[0], points[0], offset[1], points[3])?;
231
232 let mut new_points = [math::Vec::default(); 4];
233
234 new_points[0] = points[0] + normals[0] * dist;
235 new_points[3] = points[3] + normals[1] * dist;
236
237 new_points[1] =
238 line_intersection(new_points[0], new_points[0] + derive_left(points), origin, points[1])?;
239 new_points[2] =
240 line_intersection(new_points[3], new_points[3] + derive_right(points), origin, points[2])?;
241
242 Some(new_points)
243}
244
245#[derive(Clone, Debug)]
246pub enum Bezier {
247 Line([math::Vec; 2]),
248 Cubic([math::Vec; 4]),
249}
250
251impl Bezier {
252 fn is_linear(&self) -> bool {
253 match self {
254 Self::Line(_) => true,
255 Self::Cubic(points) => points
256 .iter()
257 .copied()
258 .all(|point| align(point, [points[0], points[3]]).y.abs() < TOLERANCE),
259 }
260 }
261
262 fn is_simple(&self) -> bool {
263 match self {
264 Self::Line(_) => true,
265 Self::Cubic(points) => {
266 let a0 = angle(points[0], points[3], points[1]);
267 let a1 = angle(points[0], points[3], points[2]);
268
269 if (a0 > 0.0 && a1 < 0.0 || a0 < 0.0 && a1 > 0.0) && (a1 - a0).abs() >= TOLERANCE {
270 return false;
271 }
272
273 let n0 = normal(points[0], points[1], 3.0);
274 let n1 = normal(points[2], points[3], 3.0);
275
276 if let (Some(n0), Some(n1)) = (n0, n1) {
277 let s = n0.x * n1.x + n0.y * n1.y;
278
279 s.clamp(-1.0, 1.0).acos().abs() < std::f32::consts::PI / 3.0
280 } else {
281 false
282 }
283 }
284 }
285 }
286
287 pub fn points(&self) -> &[math::Vec] {
288 match self {
289 Self::Line(points) => points,
290 Self::Cubic(points) => points,
291 }
292 }
293
294 fn points_mut(&mut self) -> &mut [math::Vec] {
295 match self {
296 Self::Line(points) => points,
297 Self::Cubic(points) => points,
298 }
299 }
300
301 pub fn left_different(&self) -> [math::Vec; 2] {
302 left_different(self.points())
303 }
304
305 pub fn right_different(&self) -> [math::Vec; 2] {
306 right_different(self.points())
307 }
308
309 pub fn normalize(&self) -> Option<Self> {
310 match *self {
311 Self::Line([p0, p1]) => {
312 if !approx_eq(p0, p1) {
313 Some(self.clone())
314 } else {
315 None
316 }
317 }
318 Self::Cubic([p0, p1, p2, p3]) => {
319 if approx_eq(p0, p1) && approx_eq(p2, p3) {
320 Self::Line([p0, p3]).normalize()
321 } else {
322 Some(self.clone())
323 }
324 }
325 }
326 }
327
328 fn as_segment(&self) -> [math::Vec; 2] {
329 match self {
330 Self::Line(points) => *points,
331 Self::Cubic([p0, _, _, p3]) => [*p0, *p3],
332 }
333 }
334
335 pub fn intersect(&self, other: &Self) -> bool {
336 line_segment_intersection(self.as_segment(), other.as_segment()).is_some()
337 }
338
339 fn map(&self, mut f: impl FnMut(math::Vec) -> math::Vec) -> Self {
340 let mut clone = self.clone();
341
342 for point in clone.points_mut() {
343 *point = f(*point);
344 }
345
346 clone
347 }
348
349 pub fn rev(&self) -> Self {
350 match self {
351 Self::Line([p0, p1]) => Self::Line([*p1, *p0]),
352 Self::Cubic([p0, p1, p2, p3]) => Self::Cubic([*p3, *p2, *p1, *p0]),
353 }
354 }
355
356 fn extrema(&self) -> SmallVec<[f32; 8]> {
357 let mut extrema: SmallVec<[f32; 8]> = SmallVec::new();
358
359 let is_unit = |t: f32| (t.is_finite() && (0.0..=1.0).contains(&t)).then(|| t.abs());
360
361 if let Self::Cubic(points) = self {
362 let d1 = derive(points);
363 let d2 = derive(&d1);
364
365 let d1x: SmallVec<[_; 3]> = d1.iter().map(|p| p.x).collect();
366 let d2x: SmallVec<[_; 2]> = d2.iter().map(|p| p.x).collect();
367 let d1y: SmallVec<[_; 3]> = d1.iter().map(|p| p.y).collect();
368 let d2y: SmallVec<[_; 2]> = d2.iter().map(|p| p.y).collect();
369
370 extrema.extend(droots(&d1x).into_iter().filter_map(is_unit));
371 extrema.extend(droots(&d2x).into_iter().filter_map(is_unit));
372 extrema.extend(droots(&d1y).into_iter().filter_map(is_unit));
373 extrema.extend(droots(&d2y).into_iter().filter_map(is_unit));
374 }
375
376 extrema.sort_by(|&a, b| a.partial_cmp(b).unwrap());
377 extrema.dedup();
378
379 extrema
380 }
381
382 fn reduce(&self) -> SmallVec<[Self; 16]> {
383 const STEP: f32 = 0.01;
384
385 let mut extrema = self.extrema();
386
387 if extrema.first().map(|&e| e <= STEP) != Some(true) {
388 extrema.insert(0, 0.0);
389 }
390
391 if extrema.last().map(|&e| (e - 1.0).abs() <= STEP) != Some(true) {
392 extrema.push(1.0);
393 }
394
395 let mut pass0: SmallVec<[_; 8]> = SmallVec::new();
396 let mut pass1 = SmallVec::new();
397
398 if let Self::Cubic(points) = self {
399 for window in extrema.windows(2) {
400 if let [t0, t1] = *window {
401 pass0.push(split(points, t0, t1));
402 }
403 }
404
405 'outer: for segment_pass0 in pass0 {
406 let mut t0 = 0.0;
407 let mut t1 = 0.0;
408
409 while t1 <= 1.0 - TOLERANCE {
410 t1 = t0 + STEP;
411 while t1 <= 1.0 + STEP - TOLERANCE {
412 let segment = split(&segment_pass0, t0, t1);
413 let bezier = Self::Cubic(segment);
414
415 if !bezier.is_simple() {
416 t1 -= STEP;
417
418 if (t1 - t0).abs() < STEP {
419 if let Some(normalized) = Self::Cubic(segment_pass0).normalize() {
420 if t0 == 0.0 {
421 pass1.push(normalized);
422 }
423 }
424 continue 'outer;
425 }
426
427 pass1.push(Self::Cubic(split(&segment_pass0, t0, t1)));
428
429 t0 = t1;
430 break;
431 }
432
433 t1 += STEP;
434 }
435 }
436
437 if t0 < 1.0 {
438 pass1.push(Self::Cubic(split(&segment_pass0, t0, 1.0)));
439 }
440 }
441 }
442
443 if pass1.is_empty() {
444 pass1.push(self.clone());
445 }
446
447 pass1
448 }
449
450 pub fn offset(&self, dist: f32) -> SmallVec<[Self; 16]> {
451 if dist.is_sign_negative() {
452 return self.rev().offset(-dist);
453 }
454
455 let mut curves = SmallVec::new();
456
457 if self.is_linear() {
458 let normal = normal_left(self.points());
459
460 curves.push(
461 self.map(|point| {
462 math::Vec::new(point.x + dist * normal.x, point.y + dist * normal.y)
463 }),
464 );
465
466 return curves;
467 }
468 self.reduce()
469 .iter()
470 .filter_map(|bezier| {
471 bezier
472 .normalize()
473 .map(|bezier| {
474 if bezier.is_linear() {
475 bezier.offset(dist)[0].clone()
476 } else if let Self::Cubic(points) = bezier {
477 if let Some(scaled) = scale(&points, dist) {
478 Self::Cubic(scaled)
479 } else {
480 Self::Line([points[0], points[3]]).offset(dist)[0].clone()
481 }
482 } else {
483 unreachable!()
484 }
485 })
486 .as_ref()
487 .and_then(Bezier::normalize)
488 })
489 .collect()
490 }
491}
492
493#[cfg(test)]
494mod tests {
495 use super::*;
496
497 macro_rules! assert_approx_eq {
498 ($a:expr, $b:expr) => {{
499 assert!(
500 ($a - $b).abs() < TOLERANCE,
501 "assertion failed: `(left !== right)` \
502 (left: `{:?}`, right: `{:?}`)",
503 $a,
504 $b,
505 );
506 }};
507 }
508
509 #[test]
510 fn offset_line() {
511 let line = Bezier::Line([math::Vec::new(-0.5, -0.5), math::Vec::new(0.5, 0.5)]);
512
513 let pos_offset = line.offset(2.0f32.sqrt() / 2.0);
514 assert_eq!(pos_offset.len(), 1);
515 assert_eq!(pos_offset[0].points().len(), 2);
516 assert_approx_eq!(pos_offset[0].points()[0].x, -1.0);
517 assert_approx_eq!(pos_offset[0].points()[0].y, 0.0);
518 assert_approx_eq!(pos_offset[0].points()[1].x, 0.0);
519 assert_approx_eq!(pos_offset[0].points()[1].y, 1.0);
520
521 let neg_offset = line.offset(-(2.0f32.sqrt()) / 2.0);
522 assert_eq!(neg_offset.len(), 1);
523 assert_eq!(neg_offset[0].points().len(), 2);
524 assert_approx_eq!(neg_offset[0].points()[1].x, 0.0);
525 assert_approx_eq!(neg_offset[0].points()[1].y, -1.0);
526 assert_approx_eq!(neg_offset[0].points()[0].x, 1.0);
527 assert_approx_eq!(neg_offset[0].points()[0].y, 0.0);
528 }
529}