aprilgrid/
math_util.rs

1use std::f32::consts::PI;
2
3use faer::linalg::solvers::Solve;
4
5pub fn find_xy(a0: f32, b0: f32, c0: f32, a1: f32, b1: f32, c1: f32) -> (f32, f32) {
6    let a = faer::mat![[a0, b0], [a1, b1]];
7    let b = faer::mat![[-c0], [-c1]];
8    let plu = a.partial_piv_lu();
9    let x1 = plu.solve(&b);
10
11    unsafe { (*x1.get_unchecked(0, 0), *x1.get_unchecked(1, 0)) }
12}
13
14/// Abs theta diff [0, 90.0]
15pub const fn theta_distance_degree(t0: f32, t1: f32) -> f32 {
16    let mut d = t0 - t1 + 90.0;
17    if d < 0.0 {
18        d += 180.0;
19    } else if d > 180.0 {
20        d -= 180.0;
21    }
22    if d > 90.0 { d - 90.0 } else { 90.0 - d }
23}
24pub const fn cross(v0: &(f32, f32), v1: &(f32, f32)) -> f32 {
25    v0.0 * v1.1 - v0.1 * v1.0
26}
27pub const fn dot(v0: &(f32, f32), v1: &(f32, f32)) -> f32 {
28    v0.0 * v1.0 + v0.1 * v1.1
29}
30
31pub fn angle_degree(v0: &(f32, f32), v1: &(f32, f32)) -> f32 {
32    (v1.1 * v0.0 - v1.0 * v0.1).atan2(v0.0 * v1.0 + v0.1 * v1.1) * 180.0 / PI
33}
34
35#[cfg(test)]
36mod tests {
37    use super::*;
38
39    #[test]
40    fn test_find_xy() {
41        // x + y = 2
42        // x - y = 0
43        // => x = 1, y = 1
44        let (x, y) = find_xy(1.0, 1.0, -2.0, 1.0, -1.0, 0.0);
45        assert!((x - 1.0).abs() < 1e-6);
46        assert!((y - 1.0).abs() < 1e-6);
47    }
48
49    #[test]
50    fn test_theta_distance_degree() {
51        assert!((theta_distance_degree(0.0, 0.0) - 0.0).abs() < 1e-6);
52        assert!((theta_distance_degree(0.0, 90.0) - 90.0).abs() < 1e-6);
53        assert!((theta_distance_degree(0.0, 45.0) - 45.0).abs() < 1e-6);
54        assert!((theta_distance_degree(0.0, 180.0) - 0.0).abs() < 1e-6); // 180 is same as 0 mod 180 for lines? No, it's [0, 90] diff
55        // Let's check implementation:
56        // d = t0 - t1 + 90.0
57        // if d < 0 => d += 180
58        // if d > 180 => d -= 180
59        // if d > 90 => d - 90 else 90 - d
60        // t0=0, t1=180 => d = -90 + 90 = -90 => d+=180 => 90. d>90? No. 90-90=0. Correct.
61
62        assert!((theta_distance_degree(10.0, 20.0) - 10.0).abs() < 1e-6);
63    }
64
65    #[test]
66    fn test_cross() {
67        let v0 = (1.0, 0.0);
68        let v1 = (0.0, 1.0);
69        assert!((cross(&v0, &v1) - 1.0).abs() < 1e-6);
70        assert!((cross(&v1, &v0) - -1.0).abs() < 1e-6);
71    }
72
73    #[test]
74    fn test_dot() {
75        let v0 = (1.0, 0.0);
76        let v1 = (0.0, 1.0);
77        assert!((dot(&v0, &v1) - 0.0).abs() < 1e-6);
78        let v2 = (1.0, 1.0);
79        assert!((dot(&v0, &v2) - 1.0).abs() < 1e-6);
80    }
81
82    #[test]
83    fn test_angle_degree() {
84        let v0 = (1.0, 0.0);
85        let v1 = (0.0, 1.0);
86        assert!((angle_degree(&v0, &v1) - 90.0).abs() < 1e-6);
87        let v2 = (1.0, 1.0);
88        assert!((angle_degree(&v0, &v2) - 45.0).abs() < 1e-6);
89    }
90}
aprilgrid/math_util.rs

aprilgrid/
math_util.rs
