kcl_lib/std/
utils.rs

1use std::f64::consts::PI;
2
3use kittycad_modeling_cmds::{shared::Angle, units::UnitLength};
4
5use super::args::TyF64;
6use crate::execution::types::NumericType;
7
8pub(crate) fn untype_point(p: [TyF64; 2]) -> ([f64; 2], NumericType) {
9    let (x, y, ty) = NumericType::combine_eq_coerce(p[0].clone(), p[1].clone(), None);
10    ([x, y], ty)
11}
12
13pub(crate) fn untype_array<const N: usize>(p: [TyF64; N]) -> ([f64; N], NumericType) {
14    let (vec, ty) = NumericType::combine_eq_array(&p);
15    (
16        vec.try_into()
17            .unwrap_or_else(|v: Vec<f64>| panic!("Expected a Vec of length {} but it was {}", N, v.len())),
18        ty,
19    )
20}
21
22pub(crate) fn point_to_mm(p: [TyF64; 2]) -> [f64; 2] {
23    [p[0].to_mm(), p[1].to_mm()]
24}
25
26pub(crate) fn untyped_point_to_mm(p: [f64; 2], units: UnitLength) -> [f64; 2] {
27    untyped_point_to_unit(p, units, UnitLength::Millimeters)
28}
29
30pub fn untyped_point_to_unit(point: [f64; 2], from_len_unit: UnitLength, to_len_unit: UnitLength) -> [f64; 2] {
31    [
32        crate::execution::types::adjust_length(from_len_unit, point[0], to_len_unit).0,
33        crate::execution::types::adjust_length(from_len_unit, point[1], to_len_unit).0,
34    ]
35}
36
37pub(crate) fn point_to_len_unit(p: [TyF64; 2], len: UnitLength) -> [f64; 2] {
38    [p[0].to_length_units(len), p[1].to_length_units(len)]
39}
40
41/// Precondition, `p` must be in `len` units (this function does no conversion).
42pub(crate) fn point_to_typed(p: [f64; 2], len: UnitLength) -> [TyF64; 2] {
43    [TyF64::new(p[0], len.into()), TyF64::new(p[1], len.into())]
44}
45
46pub(crate) fn point_3d_to_mm(p: [TyF64; 3]) -> [f64; 3] {
47    [p[0].to_mm(), p[1].to_mm(), p[2].to_mm()]
48}
49
50/// Get the distance between two points.
51pub(crate) fn distance(a: Coords2d, b: Coords2d) -> f64 {
52    ((b[0] - a[0]).powi(2) + (b[1] - a[1]).powi(2)).sqrt()
53}
54
55/// Get the angle between these points
56pub(crate) fn between(a: Coords2d, b: Coords2d) -> Angle {
57    let x = b[0] - a[0];
58    let y = b[1] - a[1];
59    normalize(Angle::from_radians(libm::atan2(y, x)))
60}
61
62/// Normalize the angle
63pub(crate) fn normalize(angle: Angle) -> Angle {
64    let deg = angle.to_degrees();
65    let result = ((deg % 360.0) + 360.0) % 360.0;
66    Angle::from_degrees(if result > 180.0 { result - 360.0 } else { result })
67}
68
69/// Gives the ▲-angle between from and to angles (shortest path)
70///
71/// Sign of the returned angle denotes direction, positive means counterClockwise 🔄
72/// # Examples
73///
74/// ```
75/// use std::f64::consts::PI;
76///
77/// use kcl_lib::std::utils::Angle;
78///
79/// assert_eq!(
80///     Angle::delta(Angle::from_radians(PI / 8.0), Angle::from_radians(PI / 4.0)),
81///     Angle::from_radians(PI / 8.0)
82/// );
83/// ```
84pub(crate) fn delta(from_angle: Angle, to_angle: Angle) -> Angle {
85    let norm_from_angle = normalize_rad(from_angle.to_radians());
86    let norm_to_angle = normalize_rad(to_angle.to_radians());
87    let provisional = norm_to_angle - norm_from_angle;
88
89    if provisional > -PI && provisional <= PI {
90        return Angle::from_radians(provisional);
91    }
92    if provisional > PI {
93        return Angle::from_radians(provisional - 2.0 * PI);
94    }
95    if provisional < -PI {
96        return Angle::from_radians(provisional + 2.0 * PI);
97    }
98    Angle::default()
99}
100
101pub(crate) fn normalize_rad(angle: f64) -> f64 {
102    let draft = angle % (2.0 * PI);
103    if draft < 0.0 { draft + 2.0 * PI } else { draft }
104}
105
106fn calculate_intersection_of_two_lines(line1: &[Coords2d; 2], line2_angle: f64, line2_point: Coords2d) -> Coords2d {
107    let line2_point_b = [
108        line2_point[0] + libm::cos(line2_angle.to_radians()) * 10.0,
109        line2_point[1] + libm::sin(line2_angle.to_radians()) * 10.0,
110    ];
111    intersect(line1[0], line1[1], line2_point, line2_point_b)
112}
113
114fn intersect(p1: Coords2d, p2: Coords2d, p3: Coords2d, p4: Coords2d) -> Coords2d {
115    let slope = |p1: Coords2d, p2: Coords2d| (p1[1] - p2[1]) / (p1[0] - p2[0]);
116    let constant = |p1: Coords2d, p2: Coords2d| p1[1] - slope(p1, p2) * p1[0];
117    let get_y = |for_x: f64, p1: Coords2d, p2: Coords2d| slope(p1, p2) * for_x + constant(p1, p2);
118
119    if p1[0] == p2[0] {
120        return [p1[0], get_y(p1[0], p3, p4)];
121    }
122    if p3[0] == p4[0] {
123        return [p3[0], get_y(p3[0], p1, p2)];
124    }
125
126    let x = (constant(p3, p4) - constant(p1, p2)) / (slope(p1, p2) - slope(p3, p4));
127    let y = get_y(x, p1, p2);
128    [x, y]
129}
130
131pub(crate) fn intersection_with_parallel_line(
132    line1: &[Coords2d; 2],
133    line1_offset: f64,
134    line2_angle: f64,
135    line2_point: Coords2d,
136) -> Coords2d {
137    calculate_intersection_of_two_lines(&offset_line(line1_offset, line1[0], line1[1]), line2_angle, line2_point)
138}
139
140fn offset_line(offset: f64, p1: Coords2d, p2: Coords2d) -> [Coords2d; 2] {
141    if p1[0] == p2[0] {
142        let direction = (p1[1] - p2[1]).signum();
143        return [[p1[0] + offset * direction, p1[1]], [p2[0] + offset * direction, p2[1]]];
144    }
145    if p1[1] == p2[1] {
146        let direction = (p2[0] - p1[0]).signum();
147        return [[p1[0], p1[1] + offset * direction], [p2[0], p2[1] + offset * direction]];
148    }
149    let x_offset = offset / libm::sin(libm::atan2(p1[1] - p2[1], p1[0] - p2[0]));
150    [[p1[0] + x_offset, p1[1]], [p2[0] + x_offset, p2[1]]]
151}
152
153pub(crate) fn get_y_component(angle: Angle, x: f64) -> Coords2d {
154    let normalised_angle = ((angle.to_degrees() % 360.0) + 360.0) % 360.0; // between 0 and 360
155    let y = x * libm::tan(normalised_angle.to_radians());
156    let sign = if normalised_angle > 90.0 && normalised_angle <= 270.0 {
157        -1.0
158    } else {
159        1.0
160    };
161    [x * sign, y * sign]
162}
163
164pub(crate) fn get_x_component(angle: Angle, y: f64) -> Coords2d {
165    let normalised_angle = ((angle.to_degrees() % 360.0) + 360.0) % 360.0; // between 0 and 360
166    let x = y / libm::tan(normalised_angle.to_radians());
167    let sign = if normalised_angle > 180.0 && normalised_angle <= 360.0 {
168        -1.0
169    } else {
170        1.0
171    };
172    [x * sign, y * sign]
173}
174
175pub(crate) fn arc_center_and_end(
176    from: Coords2d,
177    start_angle: Angle,
178    end_angle: Angle,
179    radius: f64,
180) -> (Coords2d, Coords2d) {
181    let start_angle = start_angle.to_radians();
182    let end_angle = end_angle.to_radians();
183
184    let center = [
185        -(radius * libm::cos(start_angle) - from[0]),
186        -(radius * libm::sin(start_angle) - from[1]),
187    ];
188
189    let end = [
190        center[0] + radius * libm::cos(end_angle),
191        center[1] + radius * libm::sin(end_angle),
192    ];
193
194    (center, end)
195}
196
197// Calculate the center of 3 points using an algebraic method
198// Handles if 3 points lie on the same line (collinear) by returning the average of the points (could return None instead..)
199pub(crate) fn calculate_circle_center(p1: [f64; 2], p2: [f64; 2], p3: [f64; 2]) -> [f64; 2] {
200    let (x1, y1) = (p1[0], p1[1]);
201    let (x2, y2) = (p2[0], p2[1]);
202    let (x3, y3) = (p3[0], p3[1]);
203
204    // Compute the determinant d = 2 * (x1*(y2-y3) + x2*(y3-y1) + x3*(y1-y2))
205    // Visually d is twice the area of the triangle formed by the points,
206    // also the same as: cross(p2 - p1, p3 - p1)
207    let d = 2.0 * (x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2));
208
209    // If d is nearly zero, the points are collinear, and a unique circle cannot be defined.
210    if d.abs() < f64::EPSILON {
211        return [(x1 + x2 + x3) / 3.0, (y1 + y2 + y3) / 3.0];
212    }
213
214    // squared lengths
215    let p1_sq = x1 * x1 + y1 * y1;
216    let p2_sq = x2 * x2 + y2 * y2;
217    let p3_sq = x3 * x3 + y3 * y3;
218
219    // This formula is derived from the circle equations:
220    //   (x - cx)^2 + (y - cy)^2 = r^2
221    // All 3 points will satisfy this equation, so we have 3 equations. Radius can be eliminated
222    // by subtracting one of the equations from the other two and the remaining 2 equations can
223    // be solved for cx and cy.
224    [
225        (p1_sq * (y2 - y3) + p2_sq * (y3 - y1) + p3_sq * (y1 - y2)) / d,
226        (p1_sq * (x3 - x2) + p2_sq * (x1 - x3) + p3_sq * (x2 - x1)) / d,
227    ]
228}
229
230pub struct CircleParams {
231    pub center: Coords2d,
232    pub radius: f64,
233}
234
235pub fn calculate_circle_from_3_points(points: [Coords2d; 3]) -> CircleParams {
236    let center = calculate_circle_center(points[0], points[1], points[2]);
237    CircleParams {
238        center,
239        radius: distance(center, points[1]),
240    }
241}
242
243#[cfg(test)]
244mod tests {
245    // Here you can bring your functions into scope
246    use std::f64::consts::TAU;
247
248    use approx::assert_relative_eq;
249    use pretty_assertions::assert_eq;
250
251    use super::{Angle, calculate_circle_center, get_x_component, get_y_component};
252
253    static EACH_QUAD: [(i32, [i32; 2]); 12] = [
254        (-315, [1, 1]),
255        (-225, [-1, 1]),
256        (-135, [-1, -1]),
257        (-45, [1, -1]),
258        (45, [1, 1]),
259        (135, [-1, 1]),
260        (225, [-1, -1]),
261        (315, [1, -1]),
262        (405, [1, 1]),
263        (495, [-1, 1]),
264        (585, [-1, -1]),
265        (675, [1, -1]),
266    ];
267
268    #[test]
269    fn test_get_y_component() {
270        let mut expected = Vec::new();
271        let mut results = Vec::new();
272
273        for &(angle, expected_result) in EACH_QUAD.iter() {
274            let res = get_y_component(Angle::from_degrees(angle as f64), 1.0);
275            results.push([res[0].round() as i32, res[1].round() as i32]);
276            expected.push(expected_result);
277        }
278
279        assert_eq!(results, expected);
280
281        let result = get_y_component(Angle::zero(), 1.0);
282        assert_eq!(result[0] as i32, 1);
283        assert_eq!(result[1] as i32, 0);
284
285        let result = get_y_component(Angle::from_degrees(90.0), 1.0);
286        assert_eq!(result[0] as i32, 1);
287        assert!(result[1] > 100000.0);
288
289        let result = get_y_component(Angle::from_degrees(180.0), 1.0);
290        assert_eq!(result[0] as i32, -1);
291        assert!((result[1] - 0.0).abs() < f64::EPSILON);
292
293        let result = get_y_component(Angle::from_degrees(270.0), 1.0);
294        assert_eq!(result[0] as i32, -1);
295        assert!(result[1] < -100000.0);
296    }
297
298    #[test]
299    fn test_get_x_component() {
300        let mut expected = Vec::new();
301        let mut results = Vec::new();
302
303        for &(angle, expected_result) in EACH_QUAD.iter() {
304            let res = get_x_component(Angle::from_degrees(angle as f64), 1.0);
305            results.push([res[0].round() as i32, res[1].round() as i32]);
306            expected.push(expected_result);
307        }
308
309        assert_eq!(results, expected);
310
311        let result = get_x_component(Angle::zero(), 1.0);
312        assert!(result[0] > 100000.0);
313        assert_eq!(result[1] as i32, 1);
314
315        let result = get_x_component(Angle::from_degrees(90.0), 1.0);
316        assert!((result[0] - 0.0).abs() < f64::EPSILON);
317        assert_eq!(result[1] as i32, 1);
318
319        let result = get_x_component(Angle::from_degrees(180.0), 1.0);
320        assert!(result[0] < -100000.0);
321        assert_eq!(result[1] as i32, 1);
322
323        let result = get_x_component(Angle::from_degrees(270.0), 1.0);
324        assert!((result[0] - 0.0).abs() < f64::EPSILON);
325        assert_eq!(result[1] as i32, -1);
326    }
327
328    #[test]
329    fn test_arc_center_and_end() {
330        let (center, end) = super::arc_center_and_end([0.0, 0.0], Angle::zero(), Angle::from_degrees(90.0), 1.0);
331        assert_eq!(center[0].round(), -1.0);
332        assert_eq!(center[1], 0.0);
333        assert_eq!(end[0].round(), -1.0);
334        assert_eq!(end[1], 1.0);
335
336        let (center, end) = super::arc_center_and_end([0.0, 0.0], Angle::zero(), Angle::from_degrees(180.0), 1.0);
337        assert_eq!(center[0].round(), -1.0);
338        assert_eq!(center[1], 0.0);
339        assert_eq!(end[0].round(), -2.0);
340        assert_eq!(end[1].round(), 0.0);
341
342        let (center, end) = super::arc_center_and_end([0.0, 0.0], Angle::zero(), Angle::from_degrees(180.0), 10.0);
343        assert_eq!(center[0].round(), -10.0);
344        assert_eq!(center[1], 0.0);
345        assert_eq!(end[0].round(), -20.0);
346        assert_eq!(end[1].round(), 0.0);
347    }
348
349    #[test]
350    fn test_calculate_circle_center() {
351        const EPS: f64 = 1e-4;
352
353        // Test: circle center = (4.1, 1.9)
354        let p1 = [1.0, 2.0];
355        let p2 = [4.0, 5.0];
356        let p3 = [7.0, 3.0];
357        let center = calculate_circle_center(p1, p2, p3);
358        assert_relative_eq!(center[0], 4.1, epsilon = EPS);
359        assert_relative_eq!(center[1], 1.9, epsilon = EPS);
360
361        // Tests: Generate a few circles and test its points
362        let center = [3.2, 0.7];
363        let radius_array = [0.001, 0.01, 0.6, 1.0, 5.0, 60.0, 500.0, 2000.0, 400_000.0];
364        let points_array = [[0.0, 0.33, 0.66], [0.0, 0.1, 0.2], [0.0, -0.1, 0.1], [0.0, 0.5, 0.7]];
365
366        let get_point = |radius: f64, t: f64| {
367            let angle = t * TAU;
368            [
369                center[0] + radius * libm::cos(angle),
370                center[1] + radius * libm::sin(angle),
371            ]
372        };
373
374        for radius in radius_array {
375            for point in points_array {
376                let p1 = get_point(radius, point[0]);
377                let p2 = get_point(radius, point[1]);
378                let p3 = get_point(radius, point[2]);
379                let c = calculate_circle_center(p1, p2, p3);
380                assert_relative_eq!(c[0], center[0], epsilon = EPS);
381                assert_relative_eq!(c[1], center[1], epsilon = EPS);
382            }
383        }
384
385        // Test: Equilateral triangle
386        let p1 = [0.0, 0.0];
387        let p2 = [1.0, 0.0];
388        let p3 = [0.5, 3.0_f64.sqrt() / 2.0];
389        let center = calculate_circle_center(p1, p2, p3);
390        assert_relative_eq!(center[0], 0.5, epsilon = EPS);
391        assert_relative_eq!(center[1], 1.0 / (2.0 * 3.0_f64.sqrt()), epsilon = EPS);
392
393        // Test: Collinear points (should return the average of the points)
394        let p1 = [0.0, 0.0];
395        let p2 = [1.0, 0.0];
396        let p3 = [2.0, 0.0];
397        let center = calculate_circle_center(p1, p2, p3);
398        assert_relative_eq!(center[0], 1.0, epsilon = EPS);
399        assert_relative_eq!(center[1], 0.0, epsilon = EPS);
400
401        // Test: Points forming a circle with radius = 1
402        let p1 = [0.0, 0.0];
403        let p2 = [0.0, 2.0];
404        let p3 = [2.0, 0.0];
405        let center = calculate_circle_center(p1, p2, p3);
406        assert_relative_eq!(center[0], 1.0, epsilon = EPS);
407        assert_relative_eq!(center[1], 1.0, epsilon = EPS);
408
409        // Test: Integer coordinates
410        let p1 = [0.0, 0.0];
411        let p2 = [0.0, 6.0];
412        let p3 = [6.0, 0.0];
413        let center = calculate_circle_center(p1, p2, p3);
414        assert_relative_eq!(center[0], 3.0, epsilon = EPS);
415        assert_relative_eq!(center[1], 3.0, epsilon = EPS);
416        // Verify radius (should be 3 * sqrt(2))
417        let radius = ((center[0] - p1[0]).powi(2) + (center[1] - p1[1]).powi(2)).sqrt();
418        assert_relative_eq!(radius, 3.0 * 2.0_f64.sqrt(), epsilon = EPS);
419    }
420}
421
422pub(crate) type Coords2d = [f64; 2];
423
424pub fn is_points_ccw_wasm(points: &[f64]) -> i32 {
425    // CCW is positive as that the Math convention
426
427    let mut sum = 0.0;
428    for i in 0..(points.len() / 2) {
429        let point1 = [points[2 * i], points[2 * i + 1]];
430        let point2 = [points[(2 * i + 2) % points.len()], points[(2 * i + 3) % points.len()]];
431        sum += (point2[0] + point1[0]) * (point2[1] - point1[1]);
432    }
433    sum.signum() as i32
434}
435
436pub(crate) fn is_points_ccw(points: &[Coords2d]) -> i32 {
437    let flattened_points: Vec<f64> = points.iter().flat_map(|&p| vec![p[0], p[1]]).collect();
438    is_points_ccw_wasm(&flattened_points)
439}
440
441fn get_slope(start: Coords2d, end: Coords2d) -> (f64, f64) {
442    let slope = if start[0] - end[0] == 0.0 {
443        f64::INFINITY
444    } else {
445        (start[1] - end[1]) / (start[0] - end[0])
446    };
447
448    let perp_slope = if slope == f64::INFINITY { 0.0 } else { -1.0 / slope };
449
450    (slope, perp_slope)
451}
452
453fn get_angle(point1: Coords2d, point2: Coords2d) -> f64 {
454    let delta_x = point2[0] - point1[0];
455    let delta_y = point2[1] - point1[1];
456    let angle = libm::atan2(delta_y, delta_x);
457
458    let result = if angle < 0.0 { angle + 2.0 * PI } else { angle };
459    result * (180.0 / PI)
460}
461
462fn delta_angle(from_angle: f64, to_angle: f64) -> f64 {
463    let norm_from_angle = normalize_rad(from_angle);
464    let norm_to_angle = normalize_rad(to_angle);
465    let provisional = norm_to_angle - norm_from_angle;
466
467    if provisional > -PI && provisional <= PI {
468        provisional
469    } else if provisional > PI {
470        provisional - 2.0 * PI
471    } else if provisional < -PI {
472        provisional + 2.0 * PI
473    } else {
474        provisional
475    }
476}
477
478fn deg2rad(deg: f64) -> f64 {
479    deg * (PI / 180.0)
480}
481
482fn get_mid_point(
483    center: Coords2d,
484    arc_start_point: Coords2d,
485    arc_end_point: Coords2d,
486    tan_previous_point: Coords2d,
487    radius: f64,
488    obtuse: bool,
489) -> Coords2d {
490    let angle_from_center_to_arc_start = get_angle(center, arc_start_point);
491    let angle_from_center_to_arc_end = get_angle(center, arc_end_point);
492    let delta_ang = delta_angle(
493        deg2rad(angle_from_center_to_arc_start),
494        deg2rad(angle_from_center_to_arc_end),
495    );
496    let delta_ang = delta_ang / 2.0 + deg2rad(angle_from_center_to_arc_start);
497    let shortest_arc_mid_point: Coords2d = [
498        libm::cos(delta_ang) * radius + center[0],
499        libm::sin(delta_ang) * radius + center[1],
500    ];
501    let opposite_delta = delta_ang + PI;
502    let longest_arc_mid_point: Coords2d = [
503        libm::cos(opposite_delta) * radius + center[0],
504        libm::sin(opposite_delta) * radius + center[1],
505    ];
506
507    let rotation_direction_original_points = is_points_ccw(&[tan_previous_point, arc_start_point, arc_end_point]);
508    let rotation_direction_points_on_arc = is_points_ccw(&[arc_start_point, shortest_arc_mid_point, arc_end_point]);
509    if rotation_direction_original_points != rotation_direction_points_on_arc && obtuse {
510        longest_arc_mid_point
511    } else {
512        shortest_arc_mid_point
513    }
514}
515
516fn intersect_point_n_slope(point1: Coords2d, slope1: f64, point2: Coords2d, slope2: f64) -> Coords2d {
517    let x = if slope1.abs() == f64::INFINITY {
518        point1[0]
519    } else if slope2.abs() == f64::INFINITY {
520        point2[0]
521    } else {
522        (point2[1] - slope2 * point2[0] - point1[1] + slope1 * point1[0]) / (slope1 - slope2)
523    };
524    let y = if slope1.abs() != f64::INFINITY {
525        slope1 * x - slope1 * point1[0] + point1[1]
526    } else {
527        slope2 * x - slope2 * point2[0] + point2[1]
528    };
529    [x, y]
530}
531
532/// Structure to hold input data for calculating tangential arc information.
533pub struct TangentialArcInfoInput {
534    /// The starting point of the arc.
535    pub arc_start_point: Coords2d,
536    /// The ending point of the arc.
537    pub arc_end_point: Coords2d,
538    /// The point from which the tangent is drawn.
539    pub tan_previous_point: Coords2d,
540    /// Flag to determine if the arc is obtuse. Obtuse means it flows smoothly from the previous segment.
541    pub obtuse: bool,
542}
543
544/// Structure to hold the output data from calculating tangential arc information.
545pub struct TangentialArcInfoOutput {
546    /// The center point of the arc.
547    pub center: Coords2d,
548    /// The midpoint on the arc.
549    pub arc_mid_point: Coords2d,
550    /// The radius of the arc.
551    pub radius: f64,
552    /// Start angle of the arc in radians.
553    pub start_angle: f64,
554    /// End angle of the arc in radians.
555    pub end_angle: f64,
556    /// If the arc is counter-clockwise.
557    pub ccw: i32,
558    /// The length of the arc.
559    pub arc_length: f64,
560}
561
562// tanPreviousPoint and arcStartPoint make up a straight segment leading into the arc (of which the arc should be tangential). The arc should start at arcStartPoint and end at, arcEndPoint
563// With this information we should everything we need to calculate the arc's center and radius. However there is two tangential arcs possible, that just varies on their direction
564// One is obtuse where the arc smoothly flows from the straight segment, and the other would be acute that immediately cuts back in the other direction. The obtuse boolean is there to control for this.
565pub fn get_tangential_arc_to_info(input: TangentialArcInfoInput) -> TangentialArcInfoOutput {
566    let (_, perp_slope) = get_slope(input.tan_previous_point, input.arc_start_point);
567    let tangential_line_perp_slope = perp_slope;
568
569    // Calculate the midpoint of the line segment between arcStartPoint and arcEndPoint
570    let mid_point: Coords2d = [
571        (input.arc_start_point[0] + input.arc_end_point[0]) / 2.0,
572        (input.arc_start_point[1] + input.arc_end_point[1]) / 2.0,
573    ];
574
575    let slope_mid_point_line = get_slope(input.arc_start_point, mid_point);
576
577    let center: Coords2d;
578    let radius: f64;
579
580    if tangential_line_perp_slope == slope_mid_point_line.0 {
581        // can't find the intersection of the two lines if they have the same gradient
582        // but in this case the center is the midpoint anyway
583        center = mid_point;
584        radius =
585            ((input.arc_start_point[0] - center[0]).powi(2) + (input.arc_start_point[1] - center[1]).powi(2)).sqrt();
586    } else {
587        center = intersect_point_n_slope(
588            mid_point,
589            slope_mid_point_line.1,
590            input.arc_start_point,
591            tangential_line_perp_slope,
592        );
593        radius =
594            ((input.arc_start_point[0] - center[0]).powi(2) + (input.arc_start_point[1] - center[1]).powi(2)).sqrt();
595    }
596
597    let arc_mid_point = get_mid_point(
598        center,
599        input.arc_start_point,
600        input.arc_end_point,
601        input.tan_previous_point,
602        radius,
603        input.obtuse,
604    );
605
606    let start_angle = libm::atan2(
607        input.arc_start_point[1] - center[1],
608        input.arc_start_point[0] - center[0],
609    );
610    let end_angle = libm::atan2(input.arc_end_point[1] - center[1], input.arc_end_point[0] - center[0]);
611    let ccw = is_points_ccw(&[input.arc_start_point, arc_mid_point, input.arc_end_point]);
612
613    let arc_mid_angle = libm::atan2(arc_mid_point[1] - center[1], arc_mid_point[0] - center[0]);
614    let start_to_mid_arc_length = radius
615        * delta(Angle::from_radians(start_angle), Angle::from_radians(arc_mid_angle))
616            .to_radians()
617            .abs();
618    let mid_to_end_arc_length = radius
619        * delta(Angle::from_radians(arc_mid_angle), Angle::from_radians(end_angle))
620            .to_radians()
621            .abs();
622    let arc_length = start_to_mid_arc_length + mid_to_end_arc_length;
623
624    TangentialArcInfoOutput {
625        center,
626        radius,
627        arc_mid_point,
628        start_angle,
629        end_angle,
630        ccw,
631        arc_length,
632    }
633}
634
635#[cfg(test)]
636mod get_tangential_arc_to_info_tests {
637    use approx::assert_relative_eq;
638
639    use super::*;
640
641    fn round_to_three_decimals(num: f64) -> f64 {
642        (num * 1000.0).round() / 1000.0
643    }
644
645    #[test]
646    fn test_basic_case() {
647        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
648            tan_previous_point: [0.0, -5.0],
649            arc_start_point: [0.0, 0.0],
650            arc_end_point: [4.0, 0.0],
651            obtuse: true,
652        });
653        assert_relative_eq!(result.center[0], 2.0);
654        assert_relative_eq!(result.center[1], 0.0);
655        assert_relative_eq!(result.arc_mid_point[0], 2.0);
656        assert_relative_eq!(result.arc_mid_point[1], 2.0);
657        assert_relative_eq!(result.radius, 2.0);
658        assert_relative_eq!(result.start_angle, PI);
659        assert_relative_eq!(result.end_angle, 0.0);
660        assert_eq!(result.ccw, -1);
661    }
662
663    #[test]
664    fn basic_case_with_arc_centered_at_0_0_and_the_tangential_line_being_45_degrees() {
665        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
666            tan_previous_point: [0.0, -4.0],
667            arc_start_point: [2.0, -2.0],
668            arc_end_point: [-2.0, 2.0],
669            obtuse: true,
670        });
671        assert_relative_eq!(result.center[0], 0.0);
672        assert_relative_eq!(result.center[1], 0.0);
673        assert_relative_eq!(round_to_three_decimals(result.arc_mid_point[0]), 2.0);
674        assert_relative_eq!(round_to_three_decimals(result.arc_mid_point[1]), 2.0);
675        assert_relative_eq!(result.radius, (2.0f64 * 2.0 + 2.0 * 2.0).sqrt());
676        assert_relative_eq!(result.start_angle, -PI / 4.0);
677        assert_relative_eq!(result.end_angle, 3.0 * PI / 4.0);
678        assert_eq!(result.ccw, 1);
679    }
680
681    #[test]
682    fn test_get_tangential_arc_to_info_moving_arc_end_point() {
683        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
684            tan_previous_point: [0.0, -4.0],
685            arc_start_point: [2.0, -2.0],
686            arc_end_point: [2.0, 2.0],
687            obtuse: true,
688        });
689        let expected_radius = (2.0f64 * 2.0 + 2.0 * 2.0).sqrt();
690        assert_relative_eq!(round_to_three_decimals(result.center[0]), 0.0);
691        assert_relative_eq!(result.center[1], 0.0);
692        assert_relative_eq!(result.arc_mid_point[0], expected_radius);
693        assert_relative_eq!(round_to_three_decimals(result.arc_mid_point[1]), -0.0);
694        assert_relative_eq!(result.radius, expected_radius);
695        assert_relative_eq!(result.start_angle, -PI / 4.0);
696        assert_relative_eq!(result.end_angle, PI / 4.0);
697        assert_eq!(result.ccw, 1);
698    }
699
700    #[test]
701    fn test_get_tangential_arc_to_info_moving_arc_end_point_again() {
702        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
703            tan_previous_point: [0.0, -4.0],
704            arc_start_point: [2.0, -2.0],
705            arc_end_point: [-2.0, -2.0],
706            obtuse: true,
707        });
708        let expected_radius = (2.0f64 * 2.0 + 2.0 * 2.0).sqrt();
709        assert_relative_eq!(result.center[0], 0.0);
710        assert_relative_eq!(result.center[1], 0.0);
711        assert_relative_eq!(result.radius, expected_radius);
712        assert_relative_eq!(round_to_three_decimals(result.arc_mid_point[0]), 0.0);
713        assert_relative_eq!(result.arc_mid_point[1], expected_radius);
714        assert_relative_eq!(result.start_angle, -PI / 4.0);
715        assert_relative_eq!(result.end_angle, -3.0 * PI / 4.0);
716        assert_eq!(result.ccw, 1);
717    }
718
719    #[test]
720    fn test_get_tangential_arc_to_info_acute_moving_arc_end_point() {
721        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
722            tan_previous_point: [0.0, -4.0],
723            arc_start_point: [2.0, -2.0],
724            arc_end_point: [-2.0, -2.0],
725            obtuse: false,
726        });
727        let expected_radius = (2.0f64 * 2.0 + 2.0 * 2.0).sqrt();
728        assert_relative_eq!(result.center[0], 0.0);
729        assert_relative_eq!(result.center[1], 0.0);
730        assert_relative_eq!(result.radius, expected_radius);
731        assert_relative_eq!(round_to_three_decimals(result.arc_mid_point[0]), -0.0);
732        assert_relative_eq!(result.arc_mid_point[1], -expected_radius);
733        assert_relative_eq!(result.start_angle, -PI / 4.0);
734        assert_relative_eq!(result.end_angle, -3.0 * PI / 4.0);
735        // would be cw if it was obtuse
736        assert_eq!(result.ccw, -1);
737    }
738
739    #[test]
740    fn test_get_tangential_arc_to_info_obtuse_with_wrap_around() {
741        let arc_end = libm::cos(std::f64::consts::PI / 4.0) * 2.0;
742        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
743            tan_previous_point: [2.0, -4.0],
744            arc_start_point: [2.0, 0.0],
745            arc_end_point: [0.0, -2.0],
746            obtuse: true,
747        });
748        assert_relative_eq!(result.center[0], -0.0);
749        assert_relative_eq!(result.center[1], 0.0);
750        assert_relative_eq!(result.radius, 2.0);
751        assert_relative_eq!(result.arc_mid_point[0], -arc_end);
752        assert_relative_eq!(result.arc_mid_point[1], arc_end);
753        assert_relative_eq!(result.start_angle, 0.0);
754        assert_relative_eq!(result.end_angle, -PI / 2.0);
755        assert_eq!(result.ccw, 1);
756    }
757
758    #[test]
759    fn test_arc_length_obtuse_cw() {
760        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
761            tan_previous_point: [-1.0, -1.0],
762            arc_start_point: [-1.0, 0.0],
763            arc_end_point: [0.0, -1.0],
764            obtuse: true,
765        });
766        let circumference = 2.0 * PI * result.radius;
767        let expected_length = circumference * 3.0 / 4.0; // 3 quarters of a circle circle
768        assert_relative_eq!(result.arc_length, expected_length);
769    }
770
771    #[test]
772    fn test_arc_length_acute_cw() {
773        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
774            tan_previous_point: [-1.0, -1.0],
775            arc_start_point: [-1.0, 0.0],
776            arc_end_point: [0.0, 1.0],
777            obtuse: true,
778        });
779        let circumference = 2.0 * PI * result.radius;
780        let expected_length = circumference / 4.0; // 1 quarters of a circle circle
781        assert_relative_eq!(result.arc_length, expected_length);
782    }
783
784    #[test]
785    fn test_arc_length_obtuse_ccw() {
786        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
787            tan_previous_point: [1.0, -1.0],
788            arc_start_point: [1.0, 0.0],
789            arc_end_point: [0.0, -1.0],
790            obtuse: true,
791        });
792        let circumference = 2.0 * PI * result.radius;
793        let expected_length = circumference * 3.0 / 4.0; // 1 quarters of a circle circle
794        assert_relative_eq!(result.arc_length, expected_length);
795    }
796
797    #[test]
798    fn test_arc_length_acute_ccw() {
799        let result = get_tangential_arc_to_info(TangentialArcInfoInput {
800            tan_previous_point: [1.0, -1.0],
801            arc_start_point: [1.0, 0.0],
802            arc_end_point: [0.0, 1.0],
803            obtuse: true,
804        });
805        let circumference = 2.0 * PI * result.radius;
806        let expected_length = circumference / 4.0; // 1 quarters of a circle circle
807        assert_relative_eq!(result.arc_length, expected_length);
808    }
809}
810
811pub(crate) fn get_tangent_point_from_previous_arc(
812    last_arc_center: Coords2d,
813    last_arc_ccw: bool,
814    last_arc_end: Coords2d,
815) -> Coords2d {
816    let angle_from_old_center_to_arc_start = get_angle(last_arc_center, last_arc_end);
817    let tangential_angle = angle_from_old_center_to_arc_start + if last_arc_ccw { -90.0 } else { 90.0 };
818    // What is the 10.0 constant doing???
819    [
820        libm::cos(tangential_angle.to_radians()) * 10.0 + last_arc_end[0],
821        libm::sin(tangential_angle.to_radians()) * 10.0 + last_arc_end[1],
822    ]
823}
kcl_lib/std/utils.rs

kcl_lib/std/
utils.rs
