egml_core/model/geometry/
direct_position.rs

1use crate::error::Error;
2use nalgebra::{Isometry3, Point3, Rotation3, Scale3, Transform3, Vector3};
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    pub fn new_unchecked(x: f64, y: f64, z: f64) -> Self {
60        Self { x, y, z }
61    }
62
63    /// Returns the X coordinate.
64    pub fn x(&self) -> f64 {
65        self.x
66    }
67
68    /// Returns the Y coordinate.
69    pub fn y(&self) -> f64 {
70        self.y
71    }
72
73    /// Returns the Z coordinate.
74    pub fn z(&self) -> f64 {
75        self.z
76    }
77
78    /// Returns the coordinates as a `[x, y, z]` array.
79    pub fn coords(&self) -> [f64; 3] {
80        [self.x, self.y, self.z]
81    }
82
83    /// Sets the X coordinate.
84    ///
85    /// # Errors
86    ///
87    /// Returns [`Error::NonFiniteCoordinate`] with the name `"x"` if `val` is NaN or infinite.
88    pub fn set_x(&mut self, val: f64) -> Result<(), Error> {
89        if !val.is_finite() {
90            return Err(Error::NonFiniteCoordinate {
91                axis: "x",
92                value: val,
93            });
94        }
95        self.x = val;
96        Ok(())
97    }
98
99    /// Sets the Y coordinate.
100    ///
101    /// # Errors
102    ///
103    /// Returns [`Error::NonFiniteCoordinate`] with the name `"y"` if `val` is NaN or infinite.
104    pub fn set_y(&mut self, val: f64) -> Result<(), Error> {
105        if !val.is_finite() {
106            return Err(Error::NonFiniteCoordinate {
107                axis: "y",
108                value: val,
109            });
110        }
111        self.y = val;
112        Ok(())
113    }
114
115    /// Sets the Z coordinate.
116    ///
117    /// # Errors
118    ///
119    /// Returns [`Error::NonFiniteCoordinate`] with the name `"z"` if `val` is NaN or infinite.
120    pub fn set_z(&mut self, val: f64) -> Result<(), Error> {
121        if !val.is_finite() {
122            return Err(Error::NonFiniteCoordinate {
123                axis: "z",
124                value: val,
125            });
126        }
127        self.z = val;
128        Ok(())
129    }
130
131    /// Returns a single-element list containing a reference to `self`.
132    ///
133    /// This method exists so that `DirectPosition` satisfies the same
134    /// point-iteration pattern used by multi-point geometry types.
135    pub fn points(&self) -> Vec<&DirectPosition> {
136        vec![self]
137    }
138}
139
140impl DirectPosition {
141    /// Applies a rigid-body transform (rotation + translation) to this position in place.
142    ///
143    /// `m` is a [`nalgebra::Isometry3`] — a combination of a rotation and a translation
144    /// that preserves distances and angles.
145    pub fn apply_transform(&mut self, transform: Transform3<f64>) {
146        let p: Point3<f64> = transform * Point3::new(self.x, self.y, self.z);
147        self.x = p.x;
148        self.y = p.y;
149        self.z = p.z;
150    }
151
152    /// Applies a rigid-body transform (rotation + translation) to this position in place.
153    ///
154    /// Fast path: rotates and translates directly via [`nalgebra::Isometry3`] instead of
155    /// going through a full homogeneous [`Transform3`] multiply.
156    pub fn apply_isometry(&mut self, isometry: Isometry3<f64>) {
157        let p: Point3<f64> = isometry * Point3::new(self.x, self.y, self.z);
158        self.x = p.x;
159        self.y = p.y;
160        self.z = p.z;
161    }
162
163    /// Applies a pure translation to this position in place.
164    ///
165    /// Fast path: a plain component-wise add, with no rotation or matrix math at all.
166    pub fn apply_translation(&mut self, vector: Vector3<f64>) {
167        self.x += vector.x;
168        self.y += vector.y;
169        self.z += vector.z;
170    }
171
172    /// Applies a pure rotation (about the origin) to this position in place.
173    ///
174    /// Fast path: rotates directly via [`nalgebra::Rotation3`] instead of going through a
175    /// full homogeneous [`Transform3`] multiply.
176    pub fn apply_rotation(&mut self, rotation: Rotation3<f64>) {
177        let p: Point3<f64> = rotation * Point3::new(self.x, self.y, self.z);
178        self.x = p.x;
179        self.y = p.y;
180        self.z = p.z;
181    }
182
183    /// Applies a per-axis scale to this position in place.
184    ///
185    /// Fast path: a plain component-wise multiply. Uniform scale is just
186    /// `Scale3::new(s, s, s)` at the call site.
187    pub fn apply_scale(&mut self, scale: Scale3<f64>) {
188        self.x *= scale.vector.x;
189        self.y *= scale.vector.y;
190        self.z *= scale.vector.z;
191    }
192
193    /// The position with the smallest representable coordinates `(f64::MIN, f64::MIN, f64::MIN)`.
194    pub const MIN: DirectPosition = DirectPosition {
195        x: f64::MIN,
196        y: f64::MIN,
197        z: f64::MIN,
198    };
199    /// The position with the largest representable coordinates `(f64::MAX, f64::MAX, f64::MAX)`.
200    pub const MAX: DirectPosition = DirectPosition {
201        x: f64::MAX,
202        y: f64::MAX,
203        z: f64::MAX,
204    };
205    /// The origin `(0.0, 0.0, 0.0)`.
206    pub const ORIGIN: DirectPosition = DirectPosition {
207        x: 0.0,
208        y: 0.0,
209        z: 0.0,
210    };
211}
212
213impl fmt::Display for DirectPosition {
214    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
215        write!(f, "({}, {}, {})", self.x, self.y, self.z)
216    }
217}
218
219impl From<DirectPosition> for nalgebra::Vector3<f64> {
220    fn from(item: DirectPosition) -> Self {
221        Self::new(item.x, item.y, item.z)
222    }
223}
224
225impl From<nalgebra::Vector3<f64>> for DirectPosition {
226    fn from(item: nalgebra::Vector3<f64>) -> Self {
227        Self::new(item.x, item.y, item.z).unwrap()
228    }
229}
230
231impl From<&DirectPosition> for nalgebra::Vector3<f64> {
232    fn from(item: &DirectPosition) -> Self {
233        Self::new(item.x, item.y, item.z)
234    }
235}
236
237impl From<&nalgebra::Vector3<f64>> for DirectPosition {
238    fn from(item: &nalgebra::Vector3<f64>) -> Self {
239        Self::new(item.x, item.y, item.z).unwrap()
240    }
241}
242
243impl From<DirectPosition> for nalgebra::Point3<f64> {
244    fn from(item: DirectPosition) -> Self {
245        Self::new(item.x, item.y, item.z)
246    }
247}
248
249impl From<DirectPosition> for nalgebra::Point3<f32> {
250    fn from(item: DirectPosition) -> Self {
251        Self::new(item.x as f32, item.y as f32, item.z as f32)
252    }
253}
254
255impl TryFrom<nalgebra::Point3<f64>> for DirectPosition {
256    type Error = Error;
257    fn try_from(item: nalgebra::Point3<f64>) -> Result<Self, Self::Error> {
258        Self::new(item.x, item.y, item.z)
259    }
260}
261
262impl From<DirectPosition> for parry3d_f64::math::Vector {
263    fn from(item: DirectPosition) -> Self {
264        Self::new(item.x, item.y, item.z)
265    }
266}
267
268impl From<parry3d_f64::math::Vector> for DirectPosition {
269    fn from(item: parry3d_f64::math::Vector) -> Self {
270        Self::new(item.x, item.y, item.z).expect("Should work")
271    }
272}
273
274#[cfg(test)]
275mod tests {
276    use super::*;
277    use crate::model::geometry::DirectPosition;
278    use approx::relative_eq;
279    use nalgebra::{Isometry3, Rotation3, Vector3};
280    use std::f64::consts::FRAC_PI_2;
281
282    #[test]
283    fn position_clone() {
284        let p = DirectPosition::new(1.0, 2.0, 3.0).unwrap();
285        let p2 = p;
286        assert_eq!(p, p2);
287    }
288
289    #[test]
290    fn apply_basic_transform() {
291        let mut position = DirectPosition::new(1.0, 2.0, 3.0).unwrap();
292        let isometry: Isometry3<f64> =
293            Isometry3::new(Vector3::new(-1.0, -2.0, 3.0), Default::default());
294
295        position.apply_transform(nalgebra::convert(isometry));
296
297        assert_eq!(position, DirectPosition::new(0.0, 0.0, 6.0).unwrap());
298    }
299
300    #[test]
301    fn apply_basic_translation_transform() {
302        let mut position = DirectPosition::new(1.0, 2.0, 3.0).unwrap();
303        let isometry: Isometry3<f64> =
304            Isometry3::new(Vector3::new(1.0, 1.0, 1.0), Default::default());
305
306        position.apply_transform(nalgebra::convert(isometry));
307
308        assert_eq!(position, DirectPosition::new(2.0, 3.0, 4.0).unwrap());
309    }
310
311    #[test]
312    fn apply_basic_rotation_transform() {
313        let mut position = DirectPosition::new(1.0, 1.0, 0.0).unwrap();
314        let isometry: Isometry3<f64> = Isometry3::from_parts(
315            Default::default(),
316            Rotation3::from_euler_angles(0.0, 0.0, FRAC_PI_2).into(),
317        );
318
319        position.apply_transform(nalgebra::convert(isometry));
320
321        relative_eq!(position.x(), -1.0, epsilon = f64::EPSILON);
322        relative_eq!(position.y(), 1.0, epsilon = f64::EPSILON);
323        relative_eq!(position.z(), 0.0, epsilon = f64::EPSILON);
324    }
325}
egml_core/model/geometry/direct_position.rs

egml_core/model/geometry/
direct_position.rs
