egml_core/model/geometry/
direct_position.rs

1use crate::error::Error;
2use nalgebra::{Isometry3, Point3};
3use std::fmt;
4
5/// A 3-D coordinate triple in a coordinate reference system (CRS).
6///
7/// Corresponds to `gml:DirectPositionType` in ISO 19136.  All three components
8/// must be finite; `NaN` and ±infinity are rejected at construction time.
9///
10/// # Invariant
11///
12/// `x`, `y`, and `z` are always finite `f64` values.
13#[derive(Debug, Clone, Copy, PartialEq, Default)]
14pub struct DirectPosition {
15    x: f64,
16    y: f64,
17    z: f64,
18}
19
20impl DirectPosition {
21    /// Creates a new position from Cartesian coordinates.
22    ///
23    /// # Errors
24    ///
25    /// Returns [`Error::NonFiniteCoordinate`] if any coordinate is NaN or infinite.
26    ///
27    /// # Examples
28    ///
29    /// ```rust
30    /// use egml_core::model::geometry::DirectPosition;
31    ///
32    /// let pos = DirectPosition::new(1.0, 2.0, 3.0).unwrap();
33    /// assert_eq!(pos.x(), 1.0);
34    /// assert!(DirectPosition::new(f64::NAN, 0.0, 0.0).is_err());
35    /// ```
36    pub fn new(x: f64, y: f64, z: f64) -> Result<Self, Error> {
37        if !x.is_finite() {
38            return Err(Error::NonFiniteCoordinate("x"));
39        }
40        if !y.is_finite() {
41            return Err(Error::NonFiniteCoordinate("y"));
42        }
43        if !z.is_finite() {
44            return Err(Error::NonFiniteCoordinate("z"));
45        }
46
47        Ok(Self { x, y, z })
48    }
49
50    /// Returns the X coordinate.
51    pub fn x(&self) -> f64 {
52        self.x
53    }
54
55    /// Returns the Y coordinate.
56    pub fn y(&self) -> f64 {
57        self.y
58    }
59
60    /// Returns the Z coordinate.
61    pub fn z(&self) -> f64 {
62        self.z
63    }
64
65    /// Returns the coordinates as a `[x, y, z]` array.
66    pub fn coords(&self) -> [f64; 3] {
67        [self.x, self.y, self.z]
68    }
69
70    /// Sets the X coordinate.
71    ///
72    /// # Errors
73    ///
74    /// Returns [`Error::NonFiniteCoordinate`] with the name `"x"` if `val` is NaN or infinite.
75    pub fn set_x(&mut self, val: f64) -> Result<(), Error> {
76        if !val.is_finite() {
77            return Err(Error::NonFiniteCoordinate("x"));
78        }
79        self.x = val;
80        Ok(())
81    }
82
83    /// Sets the Y coordinate.
84    ///
85    /// # Errors
86    ///
87    /// Returns [`Error::NonFiniteCoordinate`] with the name `"y"` if `val` is NaN or infinite.
88    pub fn set_y(&mut self, val: f64) -> Result<(), Error> {
89        if !val.is_finite() {
90            return Err(Error::NonFiniteCoordinate("y"));
91        }
92        self.y = val;
93        Ok(())
94    }
95
96    /// Sets the Z coordinate.
97    ///
98    /// # Errors
99    ///
100    /// Returns [`Error::NonFiniteCoordinate`] with the name `"z"` if `val` is NaN or infinite.
101    pub fn set_z(&mut self, val: f64) -> Result<(), Error> {
102        if !val.is_finite() {
103            return Err(Error::NonFiniteCoordinate("z"));
104        }
105        self.z = val;
106        Ok(())
107    }
108
109    /// Returns a single-element list containing a reference to `self`.
110    ///
111    /// This method exists so that `DirectPosition` satisfies the same
112    /// point-iteration pattern used by multi-point geometry types.
113    pub fn points(&self) -> Vec<&DirectPosition> {
114        vec![self]
115    }
116
117    /// Applies a rigid-body transform (rotation + translation) to this position in place.
118    ///
119    /// `m` is a [`nalgebra::Isometry3`] — a combination of a rotation and a translation
120    /// that preserves distances and angles.
121    pub fn apply_transform(&mut self, m: &Isometry3<f64>) {
122        let p: Point3<f64> = m * Point3::new(self.x, self.y, self.z);
123        self.x = p.x;
124        self.y = p.y;
125        self.z = p.z;
126    }
127
128    /// The position with the smallest representable coordinates `(f64::MIN, f64::MIN, f64::MIN)`.
129    pub const MIN: DirectPosition = DirectPosition {
130        x: f64::MIN,
131        y: f64::MIN,
132        z: f64::MIN,
133    };
134    /// The position with the largest representable coordinates `(f64::MAX, f64::MAX, f64::MAX)`.
135    pub const MAX: DirectPosition = DirectPosition {
136        x: f64::MAX,
137        y: f64::MAX,
138        z: f64::MAX,
139    };
140    /// The origin `(0.0, 0.0, 0.0)`.
141    pub const ORIGIN: DirectPosition = DirectPosition {
142        x: 0.0,
143        y: 0.0,
144        z: 0.0,
145    };
146}
147
148impl fmt::Display for DirectPosition {
149    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
150        write!(f, "({}, {}, {})", self.x, self.y, self.z)
151    }
152}
153
154impl From<DirectPosition> for nalgebra::Vector3<f64> {
155    fn from(item: DirectPosition) -> Self {
156        Self::new(item.x, item.y, item.z)
157    }
158}
159
160impl From<nalgebra::Vector3<f64>> for DirectPosition {
161    fn from(item: nalgebra::Vector3<f64>) -> Self {
162        Self::new(item.x, item.y, item.z).unwrap()
163    }
164}
165
166impl From<&DirectPosition> for nalgebra::Vector3<f64> {
167    fn from(item: &DirectPosition) -> Self {
168        Self::new(item.x, item.y, item.z)
169    }
170}
171
172impl From<&nalgebra::Vector3<f64>> for DirectPosition {
173    fn from(item: &nalgebra::Vector3<f64>) -> Self {
174        Self::new(item.x, item.y, item.z).unwrap()
175    }
176}
177
178impl From<DirectPosition> for nalgebra::Point3<f64> {
179    fn from(item: DirectPosition) -> Self {
180        Self::new(item.x, item.y, item.z)
181    }
182}
183
184impl From<DirectPosition> for nalgebra::Point3<f32> {
185    fn from(item: DirectPosition) -> Self {
186        Self::new(item.x as f32, item.y as f32, item.z as f32)
187    }
188}
189
190impl From<nalgebra::Point3<f64>> for DirectPosition {
191    fn from(item: nalgebra::Point3<f64>) -> Self {
192        // TODO: how to handle error?
193        Self::new(item.x, item.y, item.z).expect("Should work")
194    }
195}
196
197impl From<DirectPosition> for parry3d_f64::math::Vector {
198    fn from(item: DirectPosition) -> Self {
199        Self::new(item.x, item.y, item.z)
200    }
201}
202
203impl From<parry3d_f64::math::Vector> for DirectPosition {
204    fn from(item: parry3d_f64::math::Vector) -> Self {
205        Self::new(item.x, item.y, item.z).expect("Should work")
206    }
207}
208
209#[cfg(test)]
210mod tests {
211    use super::*;
212    use crate::model::geometry::DirectPosition;
213    use approx::relative_eq;
214    use nalgebra::{Isometry3, Rotation3, Vector3};
215    use std::f64::consts::FRAC_PI_2;
216
217    #[test]
218    fn position_clone() {
219        let p = DirectPosition::new(1.0, 2.0, 3.0).unwrap();
220        let p2 = p;
221        assert_eq!(p, p2);
222    }
223
224    #[test]
225    fn apply_basic_transform() {
226        let mut position = DirectPosition::new(1.0, 2.0, 3.0).unwrap();
227        let isometry: Isometry3<f64> =
228            Isometry3::new(Vector3::new(-1.0, -2.0, 3.0), Default::default());
229
230        position.apply_transform(&isometry);
231
232        assert_eq!(position, DirectPosition::new(0.0, 0.0, 6.0).unwrap());
233    }
234
235    #[test]
236    fn apply_basic_translation_transform() {
237        let mut position = DirectPosition::new(1.0, 2.0, 3.0).unwrap();
238        let isometry: Isometry3<f64> =
239            Isometry3::new(Vector3::new(1.0, 1.0, 1.0), Default::default());
240
241        position.apply_transform(&isometry);
242
243        assert_eq!(position, DirectPosition::new(2.0, 3.0, 4.0).unwrap());
244    }
245
246    #[test]
247    fn apply_basic_rotation_transform() {
248        let mut position = DirectPosition::new(1.0, 1.0, 0.0).unwrap();
249        let isometry: Isometry3<f64> = Isometry3::from_parts(
250            Default::default(),
251            Rotation3::from_euler_angles(0.0, 0.0, FRAC_PI_2).into(),
252        );
253
254        position.apply_transform(&isometry);
255
256        relative_eq!(position.x(), -1.0, epsilon = f64::EPSILON);
257        relative_eq!(position.y(), 1.0, epsilon = f64::EPSILON);
258        relative_eq!(position.z(), 0.0, epsilon = f64::EPSILON);
259    }
260}
egml_core/model/geometry/direct_position.rs

egml_core/model/geometry/
direct_position.rs
