ecoord_core/
transform.rs

1use crate::FrameId;
2use chrono::{DateTime, Utc};
3use nalgebra::{Isometry3, Point3, Rotation3, Translation3, UnitQuaternion, Vector3};
4use std::fmt;
5
6/// Dedicated type for an identifier of a transform.
7#[derive(Debug, Clone, PartialEq, Eq, Hash, Ord, PartialOrd)]
8pub struct TransformId {
9    pub parent_frame_id: FrameId,
10    pub child_frame_id: FrameId,
11}
12
13impl TransformId {
14    pub fn new(parent_frame_id: FrameId, child_frame_id: FrameId) -> Self {
15        assert_ne!(
16            parent_frame_id, child_frame_id,
17            "frame_id must be different from child_frame_id"
18        );
19        Self {
20            parent_frame_id,
21            child_frame_id,
22        }
23    }
24}
25
26impl fmt::Display for TransformId {
27    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
28        write!(
29            f,
30            "frame_id={} child_frame_id={}",
31            self.parent_frame_id, self.child_frame_id
32        )
33    }
34}
35
36impl From<(FrameId, FrameId)> for TransformId {
37    fn from((parent_frame_id, child_frame_id): (FrameId, FrameId)) -> Self {
38        Self::new(parent_frame_id, child_frame_id)
39    }
40}
41
42impl From<(&FrameId, &FrameId)> for TransformId {
43    fn from((parent_frame_id, child_frame_id): (&FrameId, &FrameId)) -> Self {
44        Self::new(parent_frame_id.clone(), child_frame_id.clone())
45    }
46}
47
48impl From<(&str, &str)> for TransformId {
49    fn from((parent, child): (&str, &str)) -> Self {
50        TransformId::new(FrameId::from(parent), FrameId::from(child))
51    }
52}
53
54/// A time-dependent rigid transformation in 3D.
55#[derive(Debug, Clone, Copy, PartialEq)]
56pub struct TimedTransform {
57    pub timestamp: DateTime<Utc>,
58    pub transform: Transform,
59}
60
61impl TimedTransform {
62    pub fn new(timestamp: DateTime<Utc>, transform: Transform) -> Self {
63        Self {
64            timestamp,
65            transform,
66        }
67    }
68
69    pub fn from(timestamp: DateTime<Utc>, isometry: Isometry3<f64>) -> Self {
70        Self {
71            timestamp,
72            transform: Transform::from(isometry),
73        }
74    }
75}
76
77/// A time-dependent rigid transformation in 3D.
78#[derive(Debug, Clone, Copy, PartialEq)]
79pub struct Transform {
80    pub translation: Vector3<f64>,
81    pub rotation: UnitQuaternion<f64>,
82}
83
84impl Transform {
85    pub fn new(translation: Vector3<f64>, rotation: UnitQuaternion<f64>) -> Self {
86        //let rotation = UnitQuaternion::from_quaternion(rotation);
87
88        Self {
89            translation,
90            rotation,
91        }
92    }
93
94    pub fn from(isometry: Isometry3<f64>) -> Self {
95        Self {
96            translation: isometry.translation.vector,
97            rotation: isometry.rotation,
98        }
99    }
100
101    pub fn translation(&self) -> Translation3<f64> {
102        Translation3::from(self.translation)
103    }
104
105    pub fn rotation(&self) -> Rotation3<f64> {
106        Rotation3::from(self.rotation)
107    }
108
109    pub fn isometry(&self) -> Isometry3<f64> {
110        let translation = self.translation();
111        Isometry3::from_parts(translation, self.rotation)
112    }
113
114    pub fn transform_point(&self, pt: &Point3<f64>) -> Point3<f64> {
115        let rotated_point = self.rotation().transform_point(pt);
116        let _translated_point = self.translation().transform_point(pt);
117        rotated_point
118    }
119}
ecoord_core/transform.rs

ecoord_core/
transform.rs
