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

egml_core/model/geometry/
direct_position.rs
