chalkydri_apriltags/
utils.rs

1#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
2pub(crate) enum Color {
3    Black,
4    White,
5    Other,
6}
7impl Color {
8    #[inline(always)]
9    pub fn is_black(&self) -> bool {
10        *self == Color::Black
11    }
12    #[inline(always)]
13    pub fn is_white(&self) -> bool {
14        *self == Color::White
15    }
16    #[inline(always)]
17    pub fn is_good(&self) -> bool {
18        *self != Color::Other
19    }
20}
21
22/// Calculate buffer index for an x and y, given an image width
23///
24/// # Safety
25/// `y` should be within the vertical bounds.
26#[inline(always)]
27pub(crate) const unsafe fn px(x: usize, y: usize, width: usize) -> usize {
28    y.unchecked_mul(width).unchecked_add(x)
29}
30
31/// Convert a 24-bit RGB (color) value to a 8-bit luma/brightness (grayscale) value
32#[inline(always)]
33pub(crate) fn grayscale(data: &[u8]) -> u8 {
34    if let &[r, g, b] = data {
35        // Somebody else's ideal RGB conversion values:
36        // (r as f32).mul_add(0.3, (g as f32).mul_add(0.59, (b as f32) * 0.11)) as u8
37
38        // My "works I guess" RGB conversion values:
39        // (r as f32).mul_add(0.2, (g as f32).mul_add(0.69, (b as f32) * 0.11)) as u8
40
41        // An equal mix of R, G, and B is good here, because black is the absence of light.
42        (r as f32).mul_add(0.33, (g as f32).mul_add(0.33, (b as f32) * 0.33)) as u8
43    } else {
44        panic!();
45    }
46}
47
48/// Turns p1, p2, p3... into an approximate angle
49#[rustfmt::skip]
50#[inline(always)]
51pub(crate) fn fast_angle(p: u8) -> f32 {
52    match p {
53        1  =>   0.0,
54        2  =>  22.5,
55        3  =>  45.0,
56        4  =>  67.5,
57        5  =>  90.0,
58        6  => 112.5,
59        7  => 135.0,
60        8  => 157.5,
61        9  => 180.0,
62        10 => 202.5,
63        11 => 225.0,
64        12 => 247.5,
65        13 => 270.0,
66        14 => 292.5,
67        15 => 315.0,
68        16 => 337.5,
69        _ => panic!("invalid FAST point")
70    }
71}
72
73#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
74pub(crate) enum Orientation {
75    Collinear,
76    Clockwise,
77    Counterclockwise,
78}
79
80/// Calculate the orientation
81#[inline(always)]
82pub(crate) fn orientation(
83    (px, py): (usize, usize),
84    (qx, qy): (usize, usize),
85    (rx, ry): (usize, usize),
86) -> Orientation {
87    match ((qy as i32 - py as i32) * (rx as i32 - qx as i32))
88        - ((qx as i32 - px as i32) * (ry as i32 - qy as i32))
89    {
90        //unsafe {
91        //match ((qy as i16).unchecked_sub(py as i16) * (rx as i16).unchecked_sub(qx as i16)) - ((qx as i16).unchecked_sub(px as i16) * (ry as i16).unchecked_sub(qy as i16)) {
92        0 => Orientation::Collinear,
93        i => {
94            if i > 0 {
95                Orientation::Clockwise
96            } else {
97                Orientation::Counterclockwise
98            }
99        } //}
100    }
101}
102
103/// My gift wrapping implementation
104pub(crate) struct PresentWrapper {}
105impl PresentWrapper {
106    // IDEA: I can take advantage of triangles for the early termination feature.
107    // After drawing two lines, I can find the hypotenuse using Pythag theorem.
108    // Then check that the angles are ok.
109    // Also makes it fairly trivial to guess the last point and what would be acceptable edges.
110    // 3 is a magic number.
111
112    pub fn find_convex_hull(points: &[(usize, usize)]) -> Vec<(usize, usize)> {
113        let mut hull = Vec::new();
114
115        // Find leftmost point
116        let mut l = 0;
117
118        //for (i, (x, _)) in points.into_iter().enumerate() {
119        //    if *x < points[l].0 {
120        //        l = i;
121        //    }
122        //}
123        for i in 0..points.len() {
124            if points[i].0 < points[l].0 {
125                l = i;
126            }
127        }
128
129        let mut p = l;
130        let mut q: usize;
131
132        while p != l || hull.is_empty() {
133            hull.push(points[p]);
134
135            q = (p + 1) % points.len();
136
137            //for (i, point) in points.into_iter().enumerate() {
138            //    if orientation(points[p], *point, points[q]) == Orientation::Counterclockwise {
139            //        q = i;
140            //    }
141            //}
142            for i in 0..points.len() {
143                if orientation(points[p], points[i], points[q]) == Orientation::Counterclockwise {
144                    q = i;
145                }
146            }
147
148            p = q;
149        }
150
151        hull
152    }
153}
chalkydri_apriltags/utils.rs

chalkydri_apriltags/
utils.rs
