camloc_common/
position.rs

1use super::Lerp;
2
3#[derive(Debug, Clone, Copy, PartialEq)]
4#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
5pub struct Position {
6    pub x: f64,
7    pub y: f64,
8    pub rotation: f64,
9}
10
11impl Position {
12    pub const fn new(x: f64, y: f64, rotation: f64) -> Self {
13        Position { x, y, rotation }
14    }
15
16    pub fn from_be_bytes(b: &[u8; 24]) -> Self {
17        Self::new(
18            f64::from_be_bytes([b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7]]),
19            f64::from_be_bytes([b[8], b[9], b[10], b[11], b[12], b[13], b[14], b[15]]),
20            f64::from_be_bytes([b[16], b[17], b[18], b[19], b[20], b[21], b[22], b[23]]),
21        )
22    }
23
24    pub fn to_be_bytes(&self) -> [u8; 24] {
25        let x = self.x.to_be_bytes();
26        let y = self.y.to_be_bytes();
27        let r = self.rotation.to_be_bytes();
28        [
29            x[0], x[1], x[2], x[3], x[4], x[5], x[6], x[7], y[0], y[1], y[2], y[3], y[4], y[5],
30            y[6], y[7], r[0], r[1], r[2], r[3], r[4], r[5], r[6], r[7],
31        ]
32    }
33}
34
35impl std::fmt::Display for Position {
36    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
37        write!(f, "({:.2}; {:.2}; {:.2}°)", self.x, self.y, self.rotation)
38    }
39}
40
41static CPOS: [(f64, f64); 4] = [(-1., 0.), (0., -1.), (1., 0.), (0., 1.)];
42
43pub fn calc_posotion_in_square_fov(side_length: f64, index: usize, fov: f64) -> Position {
44    debug_assert!(index < 4, "A square setup may only have 2 or 4 cameras");
45
46    let (sign_x, sign_y) = &CPOS[index];
47    let distance = get_camera_distance_in_square(side_length, fov);
48
49    Position::new(
50        sign_x * distance,
51        sign_y * distance,
52        (index as f64) * 90f64.to_radians(),
53    )
54}
55
56pub fn calc_position_in_square_distance(index: usize, distance: f64) -> Position {
57    debug_assert!(index < 4, "A square setup may only have 2 or 4 cameras");
58
59    let p = &CPOS[index];
60
61    Position::new(
62        p.0 * distance,
63        p.1 * distance,
64        (index as f64) * 90f64.to_radians(),
65    )
66}
67
68pub fn get_camera_distance_in_square(side_length: f64, fov: f64) -> f64 {
69    0.5 * side_length * (1. / (0.5 * fov).tan() + 1.)
70}
71
72impl Lerp for Position {
73    fn lerp(s: &Self, e: &Self, t: f64) -> Self {
74        Position::new(
75            f64::lerp(&s.x, &e.x, t),
76            f64::lerp(&s.y, &e.y, t),
77            f64::lerp(&s.rotation, &e.rotation, t),
78        )
79    }
80}
81
82impl std::ops::Add for Position {
83    type Output = Self;
84
85    fn add(self, rhs: Self) -> Self::Output {
86        Position {
87            x: self.x + rhs.x,
88            y: self.y + rhs.y,
89            rotation: self.rotation + rhs.rotation,
90        }
91    }
92}
93
94impl std::ops::Sub for Position {
95    type Output = Self;
96
97    fn sub(self, rhs: Self) -> Self::Output {
98        Position {
99            x: self.x - rhs.x,
100            y: self.y - rhs.y,
101            rotation: self.rotation - rhs.rotation,
102        }
103    }
104}
105
106impl std::ops::AddAssign for Position {
107    fn add_assign(&mut self, rhs: Self) {
108        self.x += rhs.x;
109        self.y += rhs.y;
110        self.rotation += rhs.rotation;
111    }
112}
113
114impl std::ops::SubAssign for Position {
115    fn sub_assign(&mut self, rhs: Self) {
116        self.x -= rhs.x;
117        self.y -= rhs.y;
118        self.rotation -= rhs.rotation;
119    }
120}
121
122impl std::ops::Mul<f64> for Position {
123    type Output = Self;
124
125    fn mul(self, rhs: f64) -> Self::Output {
126        Position {
127            x: self.x * rhs,
128            y: self.y * rhs,
129            rotation: self.rotation * rhs,
130        }
131    }
132}
133
134impl std::ops::MulAssign<f64> for Position {
135    fn mul_assign(&mut self, rhs: f64) {
136        self.x *= rhs;
137        self.y *= rhs;
138        self.rotation *= rhs;
139    }
140}
141
142impl std::iter::Sum for Position {
143    fn sum<I: Iterator<Item = Self>>(iter: I) -> Self {
144        let mut s = Position::new(0., 0., 0.);
145        for p in iter {
146            s += p;
147        }
148        s
149    }
150}
camloc_common/position.rs

camloc_common/
position.rs
