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

kcl_lib/std/
utils.rs
