kcl_lib/std/
utils.rs

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

kcl_lib/std/
utils.rs
