1 // Copyright 2022 Google LLC
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // https://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14
15 use glam::{EulerRot, Quat, Vec3};
16 use serde::ser::{Serialize, SerializeStruct, Serializer};
17 use std::default::Default;
18 use std::fmt::Display;
19
20 #[derive(Debug, Clone, Copy)]
21 pub struct Position {
22 position: Vec3,
23 rotation: Quat,
24 }
25
26 impl Display for Position {
fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result27 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
28 let (roll, pitch, yaw) = self.rotation.to_euler(EulerRot::ZXY);
29 write!(
30 f,
31 "Position: {}, {}, {} Rotation: {}, {}, {}",
32 self.position.x,
33 self.position.y,
34 self.position.z,
35 yaw.to_degrees().round(),
36 pitch.to_degrees().round(),
37 roll.to_degrees().round(),
38 )
39 }
40 }
41
42 impl Serialize for Position {
serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error> where S: Serializer,43 fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
44 where
45 S: Serializer,
46 {
47 let mut state = serializer.serialize_struct("Position", 5)?;
48 state.serialize_field("x", &(self.position.x as i16))?;
49 state.serialize_field("y", &(self.position.y as i16))?;
50 state.serialize_field("z", &(self.position.z as i16))?;
51
52 let (roll, pitch, yaw) = self.rotation.to_euler(EulerRot::ZXY);
53
54 state.serialize_field("yaw", &(yaw.to_degrees().round() as i16))?;
55 state.serialize_field("pitch", &(pitch.to_degrees().round() as i8))?;
56 state.serialize_field("roll", &(roll.to_degrees().round() as i16))?;
57 state.end()
58 }
59 }
60
checked_div(num: f32, den: f32) -> Option<f32>61 fn checked_div(num: f32, den: f32) -> Option<f32> {
62 if den == 0. {
63 None
64 } else {
65 Some(num / den)
66 }
67 }
68
azimuth(delta: Vec3) -> f3269 fn azimuth(delta: Vec3) -> f32 {
70 checked_div(delta.x, delta.z).map_or(
71 if delta.x == 0. {
72 0.
73 } else {
74 delta.x.signum() * std::f32::consts::FRAC_PI_2
75 },
76 f32::atan,
77 ) + if delta.z >= 0. {
78 0.
79 } else {
80 delta.x.signum() * std::f32::consts::PI
81 }
82 }
83
elevation(delta: Vec3) -> f3284 fn elevation(delta: Vec3) -> f32 {
85 checked_div(delta.y, f32::sqrt(delta.x.powi(2) + delta.z.powi(2)))
86 .map_or(delta.y.signum() * std::f32::consts::FRAC_PI_2, f32::atan)
87 }
88
89 impl Position {
new(x: i16, y: i16, z: i16, yaw: i16, pitch: i8, roll: i16) -> Self90 pub fn new(x: i16, y: i16, z: i16, yaw: i16, pitch: i8, roll: i16) -> Self {
91 Self {
92 position: Vec3::new(x as f32, y as f32, z as f32),
93 rotation: Quat::from_euler(
94 EulerRot::ZXY, // Rotation performed from right to left order
95 (roll as f32).to_radians(),
96 (pitch as f32).to_radians(),
97 (yaw as f32).to_radians(),
98 ),
99 }
100 }
101
compute_range_azimuth_elevation(&self, other: &Position) -> (u16, i16, i8)102 pub fn compute_range_azimuth_elevation(&self, other: &Position) -> (u16, i16, i8) {
103 let delta = other.position - self.position;
104
105 let distance = delta.length();
106 let direction = self.rotation.mul_vec3(delta);
107
108 let azimuth = azimuth(direction).to_degrees().round();
109 let elevation = elevation(direction).to_degrees().round();
110
111 assert!((-180. ..=180.).contains(&azimuth));
112 assert!((-90. ..=90.).contains(&elevation));
113
114 (
115 f32::min(distance, u16::MAX as f32) as u16,
116 azimuth as i16,
117 elevation as i8,
118 )
119 }
120 }
121
122 impl Default for Position {
default() -> Self123 fn default() -> Self {
124 Self::new(0, 0, 0, 0, 0, 0)
125 }
126 }
127
128 #[cfg(test)]
129 mod tests {
130 use super::Position;
131
132 #[test]
range()133 fn range() {
134 let position_a = Position::new(0, 0, 0, 0, 0, 0);
135 {
136 let position_b = Position::new(10, 0, 0, 0, 0, 0);
137 let (range, _, _) = position_a.compute_range_azimuth_elevation(&position_b);
138 assert!(range == 10);
139 }
140 {
141 let position_b = Position::new(-10, 0, 0, 0, 0, 0);
142 let (range, _, _) = position_a.compute_range_azimuth_elevation(&position_b);
143 assert!(range == 10);
144 }
145 {
146 let position_b = Position::new(10, 10, 0, 0, 0, 0);
147 let (range, _, _) = position_a.compute_range_azimuth_elevation(&position_b);
148 assert!(range == f32::sqrt(10. * 10. + 10. * 10.).round() as u16);
149 }
150 {
151 let position_b = Position::new(-10, -10, -10, 0, 0, 0);
152 let (range, _, _) = position_a.compute_range_azimuth_elevation(&position_b);
153 assert!(range == f32::sqrt(10. * 10. + 10. * 10. + 10. * 10.).round() as u16);
154 }
155 }
156
157 #[test]
azimuth_without_rotation()158 fn azimuth_without_rotation() {
159 let position_a = Position::new(0, 0, 0, 0, 0, 0);
160 {
161 let position_b = Position::new(10, 0, 10, 0, 0, 0);
162 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
163 assert!(azimuth == 45);
164 assert!(elevation == 0);
165 }
166 {
167 let position_b = Position::new(-10, 0, 10, 0, 0, 0);
168 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
169 assert!(azimuth == -45);
170 assert!(elevation == 0);
171 }
172 {
173 let position_b = Position::new(10, 0, -10, 0, 0, 0);
174 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
175 assert!(azimuth == 135);
176 assert!(elevation == 0);
177 }
178 {
179 let position_b = Position::new(-10, 0, -10, 0, 0, 0);
180 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
181 assert!(azimuth == -135);
182 assert!(elevation == 0);
183 }
184 }
185
186 #[test]
elevation_without_rotation()187 fn elevation_without_rotation() {
188 let position_a = Position::new(0, 0, 0, 0, 0, 0);
189 {
190 let position_b = Position::new(0, 10, 10, 0, 0, 0);
191 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
192 assert!(azimuth == 0);
193 assert!(elevation == 45);
194 }
195 {
196 let position_b = Position::new(0, -10, 10, 0, 0, 0);
197 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
198 assert!(azimuth == 0);
199 assert!(elevation == -45);
200 }
201 {
202 let position_b = Position::new(0, 10, -10, 0, 0, 0);
203 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
204 assert!(azimuth == 180 || azimuth == -180);
205 assert!(elevation == 45);
206 }
207 {
208 let position_b = Position::new(0, -10, -10, 0, 0, 0);
209 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
210 assert!(azimuth == 180 || azimuth == -180);
211 assert!(elevation == -45);
212 }
213 }
214
215 #[test]
rotation_only()216 fn rotation_only() {
217 let position_b = Position::new(0, 0, 10, 0, 0, 0);
218 {
219 let position_a = Position::new(0, 0, 0, 0, 0, 0);
220 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
221 assert!(azimuth == 0);
222 assert!(elevation == 0);
223 }
224 {
225 let position_a = Position::new(0, 0, 0, 45, 0, 0); // <=> azimuth = -45deg
226 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
227 assert!(azimuth == 45);
228 assert!(elevation == 0);
229 }
230 {
231 let position_a = Position::new(0, 0, 0, 0, 45, 0);
232 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
233 assert!(azimuth == 0);
234 assert!(elevation == -45);
235 }
236 {
237 let position_a = Position::new(0, 0, 0, 0, 0, 45);
238 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
239 assert!(azimuth == 0);
240 assert!(elevation == 0);
241 }
242 }
243
244 #[test]
rotation_only_complex_position()245 fn rotation_only_complex_position() {
246 let position_b = Position::new(10, 10, 10, 0, 0, 0);
247 {
248 let position_a = Position::new(0, 0, 0, 0, 0, 0);
249 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
250 assert!(azimuth == 45);
251 assert!(elevation == 35);
252 }
253 {
254 let position_a = Position::new(0, 0, 0, 90, 0, 0);
255 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
256 assert!(azimuth == 90 + 45);
257 assert!(elevation == 35);
258 }
259 {
260 let position_a = Position::new(0, 0, 0, 0, 90, 0);
261 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
262 assert!(azimuth == 45);
263 assert!(elevation == -35);
264 }
265 {
266 let position_a = Position::new(0, 0, 0, 0, 0, 90);
267 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
268 assert!(azimuth == -45);
269 assert!(elevation == 35);
270 }
271 {
272 let position_a = Position::new(0, 0, 0, -45, 35, 42);
273 let (_, azimuth, elevation) = position_a.compute_range_azimuth_elevation(&position_b);
274 assert!(azimuth == 0);
275 assert!(elevation == 0);
276 }
277 }
278 }
279