1use astrs_data::{AstrsMessage, DataType, Field, RecordBatch, Result};
34
35use super::{build, read, single_row_column};
36
37#[derive(Debug, Clone, Copy, Default, PartialEq, PartialOrd)]
39pub struct Vector3 {
40 pub x: f64,
42 pub y: f64,
44 pub z: f64,
46}
47
48impl Vector3 {
49 pub const ZERO: Self = Self {
51 x: 0.0,
52 y: 0.0,
53 z: 0.0,
54 };
55
56 #[must_use]
58 pub const fn new(x: f64, y: f64, z: f64) -> Self {
59 Self { x, y, z }
60 }
61
62 #[must_use]
64 pub fn norm(self) -> f64 {
65 self.x.hypot(self.y).hypot(self.z)
66 }
67
68 #[must_use]
70 pub const fn to_array(self) -> [f64; 3] {
71 [self.x, self.y, self.z]
72 }
73
74 #[must_use]
76 pub const fn from_array(values: [f64; 3]) -> Self {
77 Self::new(values[0], values[1], values[2])
78 }
79
80 #[must_use]
83 pub fn layout() -> DataType {
84 DataType::strukt([
85 Field::required("x", DataType::Float64),
86 Field::required("y", DataType::Float64),
87 Field::required("z", DataType::Float64),
88 ])
89 }
90
91 pub fn column(values: &[Self]) -> Result<astrs_data::ArrayRef> {
97 let x: Vec<f64> = values.iter().map(|value| value.x).collect();
98 let y: Vec<f64> = values.iter().map(|value| value.y).collect();
99 let z: Vec<f64> = values.iter().map(|value| value.z).collect();
100 build::structure(vec![
101 ("x", build::primitive::<f64>(&x)),
102 ("y", build::primitive::<f64>(&y)),
103 ("z", build::primitive::<f64>(&z)),
104 ])
105 }
106
107 pub fn at(column: &astrs_data::ArrayRef, row: usize) -> Result<Self> {
113 let strukt = read::structure(column)?;
114 Ok(Self {
115 x: read::f64_at(read::field(strukt, "x")?, row)?,
116 y: read::f64_at(read::field(strukt, "y")?, row)?,
117 z: read::f64_at(read::field(strukt, "z")?, row)?,
118 })
119 }
120}
121
122impl From<[f64; 3]> for Vector3 {
123 fn from(values: [f64; 3]) -> Self {
124 Self::from_array(values)
125 }
126}
127
128impl AstrsMessage for Vector3 {
129 const URN: &'static str = "std/geometry/v1/Vector3";
130
131 fn data_type() -> DataType {
132 Self::layout()
133 }
134
135 fn to_record_batch(&self) -> Result<RecordBatch> {
136 Ok(RecordBatch::from_payload(Self::column(
137 core::slice::from_ref(self),
138 )?))
139 }
140
141 fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
142 Self::at(single_row_column(batch)?, 0)
143 }
144}
145
146super::impl_from_payload!(Vector3);
147
148#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
150pub struct Quaternion {
151 pub x: f64,
153 pub y: f64,
155 pub z: f64,
157 pub w: f64,
159}
160
161impl Quaternion {
162 pub const IDENTITY: Self = Self {
164 x: 0.0,
165 y: 0.0,
166 z: 0.0,
167 w: 1.0,
168 };
169
170 #[must_use]
172 pub const fn new(x: f64, y: f64, z: f64, w: f64) -> Self {
173 Self { x, y, z, w }
174 }
175
176 #[must_use]
178 pub const fn identity() -> Self {
179 Self::IDENTITY
180 }
181
182 #[must_use]
184 pub fn norm(self) -> f64 {
185 self.x.hypot(self.y).hypot(self.z).hypot(self.w)
186 }
187
188 #[must_use]
190 pub fn is_normalized(self, tolerance: f64) -> bool {
191 (self.norm() - 1.0).abs() <= tolerance
192 }
193
194 #[must_use]
197 pub fn normalized(self) -> Self {
198 let norm = self.norm();
199 if norm == 0.0 || !norm.is_finite() {
200 return Self::IDENTITY;
201 }
202 Self::new(self.x / norm, self.y / norm, self.z / norm, self.w / norm)
203 }
204
205 #[must_use]
207 pub const fn to_array(self) -> [f64; 4] {
208 [self.x, self.y, self.z, self.w]
209 }
210
211 #[must_use]
213 pub fn layout() -> DataType {
214 DataType::strukt([
215 Field::required("x", DataType::Float64),
216 Field::required("y", DataType::Float64),
217 Field::required("z", DataType::Float64),
218 Field::required("w", DataType::Float64),
219 ])
220 }
221
222 pub fn column(values: &[Self]) -> Result<astrs_data::ArrayRef> {
228 let x: Vec<f64> = values.iter().map(|value| value.x).collect();
229 let y: Vec<f64> = values.iter().map(|value| value.y).collect();
230 let z: Vec<f64> = values.iter().map(|value| value.z).collect();
231 let w: Vec<f64> = values.iter().map(|value| value.w).collect();
232 build::structure(vec![
233 ("x", build::primitive::<f64>(&x)),
234 ("y", build::primitive::<f64>(&y)),
235 ("z", build::primitive::<f64>(&z)),
236 ("w", build::primitive::<f64>(&w)),
237 ])
238 }
239
240 pub fn at(column: &astrs_data::ArrayRef, row: usize) -> Result<Self> {
246 let strukt = read::structure(column)?;
247 Ok(Self {
248 x: read::f64_at(read::field(strukt, "x")?, row)?,
249 y: read::f64_at(read::field(strukt, "y")?, row)?,
250 z: read::f64_at(read::field(strukt, "z")?, row)?,
251 w: read::f64_at(read::field(strukt, "w")?, row)?,
252 })
253 }
254}
255
256impl Default for Quaternion {
257 fn default() -> Self {
258 Self::IDENTITY
259 }
260}
261
262impl AstrsMessage for Quaternion {
263 const URN: &'static str = "std/geometry/v1/Quaternion";
264
265 fn data_type() -> DataType {
266 Self::layout()
267 }
268
269 fn to_record_batch(&self) -> Result<RecordBatch> {
270 Ok(RecordBatch::from_payload(Self::column(
271 core::slice::from_ref(self),
272 )?))
273 }
274
275 fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
276 Self::at(single_row_column(batch)?, 0)
277 }
278}
279
280super::impl_from_payload!(Quaternion);
281
282macro_rules! composite {
285 (
286 $(#[$meta:meta])*
287 $name:ident, $urn:literal,
288 $first:ident : $first_ty:ty = $first_name:literal,
289 $second:ident : $second_ty:ty = $second_name:literal
290 ) => {
291 $(#[$meta])*
292 #[derive(Debug, Clone, Copy, Default, PartialEq, PartialOrd)]
293 pub struct $name {
294 #[doc = concat!("The `", $first_name, "` component.")]
295 pub $first: $first_ty,
296 #[doc = concat!("The `", $second_name, "` component.")]
297 pub $second: $second_ty,
298 }
299
300 impl $name {
301 #[must_use]
303 pub const fn new($first: $first_ty, $second: $second_ty) -> Self {
304 Self { $first, $second }
305 }
306
307 #[must_use]
309 pub fn layout() -> DataType {
310 DataType::strukt([
311 Field::required($first_name, <$first_ty>::layout()),
312 Field::required($second_name, <$second_ty>::layout()),
313 ])
314 }
315
316 pub fn column(values: &[Self]) -> Result<astrs_data::ArrayRef> {
322 let first: Vec<$first_ty> = values.iter().map(|value| value.$first).collect();
323 let second: Vec<$second_ty> = values.iter().map(|value| value.$second).collect();
324 build::structure(vec![
325 ($first_name, <$first_ty>::column(&first)?),
326 ($second_name, <$second_ty>::column(&second)?),
327 ])
328 }
329
330 pub fn at(column: &astrs_data::ArrayRef, row: usize) -> Result<Self> {
337 let strukt = read::structure(column)?;
338 Ok(Self {
339 $first: <$first_ty>::at(read::field(strukt, $first_name)?, row)?,
340 $second: <$second_ty>::at(read::field(strukt, $second_name)?, row)?,
341 })
342 }
343 }
344
345 impl AstrsMessage for $name {
346 const URN: &'static str = $urn;
347
348 fn data_type() -> DataType {
349 Self::layout()
350 }
351
352 fn to_record_batch(&self) -> Result<RecordBatch> {
353 Ok(RecordBatch::from_payload(Self::column(
354 core::slice::from_ref(self),
355 )?))
356 }
357
358 fn from_record_batch(batch: &RecordBatch) -> Result<Self> {
359 Self::at(single_row_column(batch)?, 0)
360 }
361 }
362
363 super::impl_from_payload!($name);
364 };
365}
366
367composite!(
368 Pose,
370 "std/geometry/v1/Pose",
371 position: Vector3 = "position",
372 orientation: Quaternion = "orientation"
373);
374
375composite!(
376 Transform,
382 "std/geometry/v1/Transform",
383 translation: Vector3 = "translation",
384 rotation: Quaternion = "rotation"
385);
386
387composite!(
388 Twist,
390 "std/geometry/v1/Twist",
391 linear: Vector3 = "linear",
392 angular: Vector3 = "angular"
393);
394
395composite!(
396 Accel,
400 "std/geometry/v1/Accel",
401 linear: Vector3 = "linear",
402 angular: Vector3 = "angular"
403);
404
405#[cfg(test)]
406mod tests {
407 #![allow(clippy::unwrap_used, clippy::expect_used, clippy::panic)]
408
409 use super::*;
410 use crate::message::assert_registry_layout;
411
412 #[test]
413 fn every_geometry_type_conforms_to_the_registry() {
414 assert_registry_layout::<Vector3>().unwrap();
415 assert_registry_layout::<Quaternion>().unwrap();
416 assert_registry_layout::<Pose>().unwrap();
417 assert_registry_layout::<Transform>().unwrap();
418 assert_registry_layout::<Twist>().unwrap();
419 assert_registry_layout::<Accel>().unwrap();
420 }
421
422 #[test]
423 fn vectors_round_trip() {
424 let value = Vector3::new(1.5, -2.5, 3.5);
425 let batch = value.to_record_batch().unwrap();
426 assert_eq!(batch.num_rows(), 1);
427 assert_eq!(Vector3::from_record_batch(&batch).unwrap(), value);
428 assert_eq!(value.to_array(), [1.5, -2.5, 3.5]);
429 assert_eq!(Vector3::from([1.5, -2.5, 3.5]), value);
430 assert_eq!(Vector3::ZERO.norm(), 0.0);
431 assert_eq!(Vector3::new(3.0, 4.0, 0.0).norm(), 5.0);
432 assert_eq!(Vector3::default(), Vector3::ZERO);
433 }
434
435 #[test]
436 fn quaternions_round_trip_and_normalise() {
437 let half = core::f64::consts::FRAC_1_SQRT_2;
438 let value = Quaternion::new(0.0, 0.0, half, half);
439 let batch = value.to_record_batch().unwrap();
440 assert_eq!(Quaternion::from_record_batch(&batch).unwrap(), value);
441 assert!(value.is_normalized(1e-12));
442 assert_eq!(Quaternion::default(), Quaternion::IDENTITY);
443 assert_eq!(Quaternion::identity().to_array(), [0.0, 0.0, 0.0, 1.0]);
444
445 let scaled = Quaternion::new(0.0, 0.0, 0.0, 4.0);
446 assert!(!scaled.is_normalized(1e-9));
447 assert!(scaled.normalized().is_normalized(1e-12));
448
449 let zero = Quaternion::new(0.0, 0.0, 0.0, 0.0);
452 assert_eq!(zero.normalized(), Quaternion::IDENTITY);
453 let nan = Quaternion::new(f64::NAN, 0.0, 0.0, 0.0);
454 assert_eq!(nan.normalized(), Quaternion::IDENTITY);
455 }
456
457 #[test]
458 fn composites_round_trip() {
459 let pose = Pose::new(Vector3::new(1.0, 2.0, 3.0), Quaternion::identity());
460 assert_eq!(
461 Pose::from_record_batch(&pose.to_record_batch().unwrap()).unwrap(),
462 pose
463 );
464
465 let transform = Transform::new(Vector3::new(-1.0, 0.0, 0.5), Quaternion::identity());
466 assert_eq!(
467 Transform::from_record_batch(&transform.to_record_batch().unwrap()).unwrap(),
468 transform
469 );
470
471 let twist = Twist::new(Vector3::new(1.0, 0.0, 0.0), Vector3::new(0.0, 0.0, 0.2));
472 assert_eq!(
473 Twist::from_record_batch(&twist.to_record_batch().unwrap()).unwrap(),
474 twist
475 );
476
477 let accel = Accel::new(Vector3::new(0.0, 0.0, -9.81), Vector3::ZERO);
478 assert_eq!(
479 Accel::from_record_batch(&accel.to_record_batch().unwrap()).unwrap(),
480 accel
481 );
482 }
483
484 #[test]
485 fn twist_and_accel_share_a_layout_but_not_a_urn() {
486 assert_eq!(Twist::data_type(), Accel::data_type());
487 assert_ne!(<Twist as AstrsMessage>::URN, <Accel as AstrsMessage>::URN);
488 }
489
490 #[test]
491 fn multi_row_columns_hold_every_value() {
492 let poses = [
493 Pose::new(Vector3::new(0.0, 0.0, 0.0), Quaternion::identity()),
494 Pose::new(Vector3::new(1.0, 1.0, 1.0), Quaternion::identity()),
495 Pose::new(Vector3::new(2.0, 4.0, 8.0), Quaternion::identity()),
496 ];
497 let column = Pose::column(&poses).unwrap();
498 assert_eq!(column.len(), 3);
499 for (row, expected) in poses.iter().enumerate() {
500 assert_eq!(&Pose::at(&column, row).unwrap(), expected);
501 }
502 assert!(Pose::at(&column, 3).is_err());
503 }
504
505 #[test]
506 fn a_wrong_layout_is_refused_rather_than_reinterpreted() {
507 let vector = Vector3::new(1.0, 2.0, 3.0).to_record_batch().unwrap();
508 assert!(Quaternion::from_record_batch(&vector).is_err());
509 assert!(Pose::from_record_batch(&vector).is_err());
510
511 let scalar = crate::message::Scalar::from(1.0_f64)
512 .to_record_batch()
513 .unwrap();
514 assert!(Vector3::from_record_batch(&scalar).is_err());
515 }
516}