u_nesting_core/
transform.rs

1//! Transform types for 2D and 3D coordinate transformations.
2
3use u_geometry::nalgebra_types::{
4    Isometry2, Isometry3, NaPoint2 as Point2, NaPoint3 as Point3, NaVector2 as Vector2,
5    NaVector3 as Vector3, RealField, Rotation3,
6};
7
8#[cfg(feature = "serde")]
9use serde::{Deserialize, Serialize};
10
11/// A 2D rigid transformation (rotation + translation).
12#[derive(Debug, Clone, Copy, PartialEq)]
13#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
14pub struct Transform2D<S: RealField + Copy> {
15    /// Translation in x direction.
16    pub tx: S,
17    /// Translation in y direction.
18    pub ty: S,
19    /// Rotation angle in radians.
20    pub angle: S,
21}
22
23impl<S: RealField + Copy> Transform2D<S> {
24    /// Creates a new identity transform.
25    pub fn identity() -> Self {
26        Self {
27            tx: S::zero(),
28            ty: S::zero(),
29            angle: S::zero(),
30        }
31    }
32
33    /// Creates a new transform with translation only.
34    pub fn translation(tx: S, ty: S) -> Self {
35        Self {
36            tx,
37            ty,
38            angle: S::zero(),
39        }
40    }
41
42    /// Creates a new transform with rotation only.
43    pub fn rotation(angle: S) -> Self {
44        Self {
45            tx: S::zero(),
46            ty: S::zero(),
47            angle,
48        }
49    }
50
51    /// Creates a new transform with both translation and rotation.
52    pub fn new(tx: S, ty: S, angle: S) -> Self {
53        Self { tx, ty, angle }
54    }
55
56    /// Converts to a nalgebra Isometry2.
57    pub fn to_isometry(&self) -> Isometry2<S> {
58        Isometry2::new(Vector2::new(self.tx, self.ty), self.angle)
59    }
60
61    /// Creates from a nalgebra Isometry2.
62    pub fn from_isometry(iso: &Isometry2<S>) -> Self {
63        Self {
64            tx: iso.translation.x,
65            ty: iso.translation.y,
66            angle: iso.rotation.angle(),
67        }
68    }
69
70    /// Transforms a 2D point.
71    pub fn transform_point(&self, x: S, y: S) -> (S, S) {
72        let iso = self.to_isometry();
73        let p = iso.transform_point(&Point2::new(x, y));
74        (p.x, p.y)
75    }
76
77    /// Transforms a vector of 2D points.
78    pub fn transform_points(&self, points: &[(S, S)]) -> Vec<(S, S)> {
79        let iso = self.to_isometry();
80        points
81            .iter()
82            .map(|(x, y)| {
83                let p = iso.transform_point(&Point2::new(*x, *y));
84                (p.x, p.y)
85            })
86            .collect()
87    }
88
89    /// Composes two transforms: self then other.
90    pub fn then(&self, other: &Self) -> Self {
91        let iso1 = self.to_isometry();
92        let iso2 = other.to_isometry();
93        Self::from_isometry(&(iso1 * iso2))
94    }
95
96    /// Returns the inverse transform.
97    pub fn inverse(&self) -> Self {
98        Self::from_isometry(&self.to_isometry().inverse())
99    }
100
101    /// Checks if this is approximately an identity transform.
102    pub fn is_identity(&self, epsilon: S) -> bool {
103        self.tx.abs() < epsilon && self.ty.abs() < epsilon && self.angle.abs() < epsilon
104    }
105}
106
107impl<S: RealField + Copy> Default for Transform2D<S> {
108    fn default() -> Self {
109        Self::identity()
110    }
111}
112
113/// A 3D rigid transformation (rotation + translation).
114#[derive(Debug, Clone, Copy, PartialEq)]
115#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
116pub struct Transform3D<S: RealField + Copy> {
117    /// Translation in x direction.
118    pub tx: S,
119    /// Translation in y direction.
120    pub ty: S,
121    /// Translation in z direction.
122    pub tz: S,
123    /// Rotation around x axis (roll) in radians.
124    pub rx: S,
125    /// Rotation around y axis (pitch) in radians.
126    pub ry: S,
127    /// Rotation around z axis (yaw) in radians.
128    pub rz: S,
129}
130
131impl<S: RealField + Copy> Transform3D<S> {
132    /// Creates a new identity transform.
133    pub fn identity() -> Self {
134        Self {
135            tx: S::zero(),
136            ty: S::zero(),
137            tz: S::zero(),
138            rx: S::zero(),
139            ry: S::zero(),
140            rz: S::zero(),
141        }
142    }
143
144    /// Creates a new transform with translation only.
145    pub fn translation(tx: S, ty: S, tz: S) -> Self {
146        Self {
147            tx,
148            ty,
149            tz,
150            rx: S::zero(),
151            ry: S::zero(),
152            rz: S::zero(),
153        }
154    }
155
156    /// Creates a new transform with rotation only (Euler angles XYZ).
157    pub fn rotation(rx: S, ry: S, rz: S) -> Self {
158        Self {
159            tx: S::zero(),
160            ty: S::zero(),
161            tz: S::zero(),
162            rx,
163            ry,
164            rz,
165        }
166    }
167
168    /// Creates a new transform with both translation and rotation.
169    pub fn new(tx: S, ty: S, tz: S, rx: S, ry: S, rz: S) -> Self {
170        Self {
171            tx,
172            ty,
173            tz,
174            rx,
175            ry,
176            rz,
177        }
178    }
179
180    /// Converts to a nalgebra Isometry3.
181    pub fn to_isometry(&self) -> Isometry3<S> {
182        let rotation = Rotation3::from_euler_angles(self.rx, self.ry, self.rz);
183        Isometry3::from_parts(
184            Vector3::new(self.tx, self.ty, self.tz).into(),
185            rotation.into(),
186        )
187    }
188
189    /// Creates from a nalgebra Isometry3.
190    pub fn from_isometry(iso: &Isometry3<S>) -> Self {
191        let (rx, ry, rz) = iso.rotation.euler_angles();
192        Self {
193            tx: iso.translation.x,
194            ty: iso.translation.y,
195            tz: iso.translation.z,
196            rx,
197            ry,
198            rz,
199        }
200    }
201
202    /// Transforms a 3D point.
203    pub fn transform_point(&self, x: S, y: S, z: S) -> (S, S, S) {
204        let iso = self.to_isometry();
205        let p = iso.transform_point(&Point3::new(x, y, z));
206        (p.x, p.y, p.z)
207    }
208
209    /// Transforms a vector of 3D points.
210    pub fn transform_points(&self, points: &[(S, S, S)]) -> Vec<(S, S, S)> {
211        let iso = self.to_isometry();
212        points
213            .iter()
214            .map(|(x, y, z)| {
215                let p = iso.transform_point(&Point3::new(*x, *y, *z));
216                (p.x, p.y, p.z)
217            })
218            .collect()
219    }
220
221    /// Composes two transforms: self then other.
222    pub fn then(&self, other: &Self) -> Self {
223        let iso1 = self.to_isometry();
224        let iso2 = other.to_isometry();
225        Self::from_isometry(&(iso1 * iso2))
226    }
227
228    /// Returns the inverse transform.
229    pub fn inverse(&self) -> Self {
230        Self::from_isometry(&self.to_isometry().inverse())
231    }
232
233    /// Checks if this is approximately an identity transform.
234    pub fn is_identity(&self, epsilon: S) -> bool {
235        self.tx.abs() < epsilon
236            && self.ty.abs() < epsilon
237            && self.tz.abs() < epsilon
238            && self.rx.abs() < epsilon
239            && self.ry.abs() < epsilon
240            && self.rz.abs() < epsilon
241    }
242}
243
244impl<S: RealField + Copy> Default for Transform3D<S> {
245    fn default() -> Self {
246        Self::identity()
247    }
248}
249
250/// Axis-aligned bounding box in 2D.
251#[derive(Debug, Clone, Copy, PartialEq)]
252#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
253pub struct AABB2D<S> {
254    /// Minimum x coordinate.
255    pub min_x: S,
256    /// Minimum y coordinate.
257    pub min_y: S,
258    /// Maximum x coordinate.
259    pub max_x: S,
260    /// Maximum y coordinate.
261    pub max_y: S,
262}
263
264impl<S: RealField + Copy> AABB2D<S> {
265    /// Creates a new AABB from min/max coordinates.
266    pub fn new(min_x: S, min_y: S, max_x: S, max_y: S) -> Self {
267        Self {
268            min_x,
269            min_y,
270            max_x,
271            max_y,
272        }
273    }
274
275    /// Creates an AABB from a set of points.
276    pub fn from_points(points: &[(S, S)]) -> Option<Self> {
277        if points.is_empty() {
278            return None;
279        }
280
281        let mut min_x = points[0].0;
282        let mut min_y = points[0].1;
283        let mut max_x = points[0].0;
284        let mut max_y = points[0].1;
285
286        for (x, y) in points.iter().skip(1) {
287            min_x = min_x.min(*x);
288            min_y = min_y.min(*y);
289            max_x = max_x.max(*x);
290            max_y = max_y.max(*y);
291        }
292
293        Some(Self {
294            min_x,
295            min_y,
296            max_x,
297            max_y,
298        })
299    }
300
301    /// Returns the width of the AABB.
302    pub fn width(&self) -> S {
303        self.max_x - self.min_x
304    }
305
306    /// Returns the height of the AABB.
307    pub fn height(&self) -> S {
308        self.max_y - self.min_y
309    }
310
311    /// Returns the area of the AABB.
312    pub fn area(&self) -> S {
313        self.width() * self.height()
314    }
315
316    /// Returns the center point of the AABB.
317    pub fn center(&self) -> (S, S) {
318        let two = S::one() + S::one();
319        (
320            (self.min_x + self.max_x) / two,
321            (self.min_y + self.max_y) / two,
322        )
323    }
324
325    /// Checks if this AABB contains a point.
326    pub fn contains_point(&self, x: S, y: S) -> bool {
327        x >= self.min_x && x <= self.max_x && y >= self.min_y && y <= self.max_y
328    }
329
330    /// Checks if this AABB intersects another AABB.
331    pub fn intersects(&self, other: &Self) -> bool {
332        self.min_x <= other.max_x
333            && self.max_x >= other.min_x
334            && self.min_y <= other.max_y
335            && self.max_y >= other.min_y
336    }
337
338    /// Returns the intersection of two AABBs, if any.
339    pub fn intersection(&self, other: &Self) -> Option<Self> {
340        if !self.intersects(other) {
341            return None;
342        }
343
344        Some(Self {
345            min_x: self.min_x.max(other.min_x),
346            min_y: self.min_y.max(other.min_y),
347            max_x: self.max_x.min(other.max_x),
348            max_y: self.max_y.min(other.max_y),
349        })
350    }
351
352    /// Returns the union (bounding box) of two AABBs.
353    pub fn union(&self, other: &Self) -> Self {
354        Self {
355            min_x: self.min_x.min(other.min_x),
356            min_y: self.min_y.min(other.min_y),
357            max_x: self.max_x.max(other.max_x),
358            max_y: self.max_y.max(other.max_y),
359        }
360    }
361
362    /// Expands the AABB by a margin on all sides.
363    pub fn expand(&self, margin: S) -> Self {
364        Self {
365            min_x: self.min_x - margin,
366            min_y: self.min_y - margin,
367            max_x: self.max_x + margin,
368            max_y: self.max_y + margin,
369        }
370    }
371}
372
373/// Axis-aligned bounding box in 3D.
374#[derive(Debug, Clone, Copy, PartialEq)]
375#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
376pub struct AABB3D<S> {
377    /// Minimum x coordinate.
378    pub min_x: S,
379    /// Minimum y coordinate.
380    pub min_y: S,
381    /// Minimum z coordinate.
382    pub min_z: S,
383    /// Maximum x coordinate.
384    pub max_x: S,
385    /// Maximum y coordinate.
386    pub max_y: S,
387    /// Maximum z coordinate.
388    pub max_z: S,
389}
390
391impl<S: RealField + Copy> AABB3D<S> {
392    /// Creates a new AABB from min/max coordinates.
393    pub fn new(min_x: S, min_y: S, min_z: S, max_x: S, max_y: S, max_z: S) -> Self {
394        Self {
395            min_x,
396            min_y,
397            min_z,
398            max_x,
399            max_y,
400            max_z,
401        }
402    }
403
404    /// Creates an AABB from a set of points.
405    pub fn from_points(points: &[(S, S, S)]) -> Option<Self> {
406        if points.is_empty() {
407            return None;
408        }
409
410        let mut min_x = points[0].0;
411        let mut min_y = points[0].1;
412        let mut min_z = points[0].2;
413        let mut max_x = points[0].0;
414        let mut max_y = points[0].1;
415        let mut max_z = points[0].2;
416
417        for (x, y, z) in points.iter().skip(1) {
418            min_x = min_x.min(*x);
419            min_y = min_y.min(*y);
420            min_z = min_z.min(*z);
421            max_x = max_x.max(*x);
422            max_y = max_y.max(*y);
423            max_z = max_z.max(*z);
424        }
425
426        Some(Self {
427            min_x,
428            min_y,
429            min_z,
430            max_x,
431            max_y,
432            max_z,
433        })
434    }
435
436    /// Returns the width (x dimension) of the AABB.
437    pub fn width(&self) -> S {
438        self.max_x - self.min_x
439    }
440
441    /// Returns the depth (y dimension) of the AABB.
442    pub fn depth(&self) -> S {
443        self.max_y - self.min_y
444    }
445
446    /// Returns the height (z dimension) of the AABB.
447    pub fn height(&self) -> S {
448        self.max_z - self.min_z
449    }
450
451    /// Returns the volume of the AABB.
452    pub fn volume(&self) -> S {
453        self.width() * self.depth() * self.height()
454    }
455
456    /// Returns the center point of the AABB.
457    pub fn center(&self) -> (S, S, S) {
458        let two = S::one() + S::one();
459        (
460            (self.min_x + self.max_x) / two,
461            (self.min_y + self.max_y) / two,
462            (self.min_z + self.max_z) / two,
463        )
464    }
465
466    /// Checks if this AABB contains a point.
467    pub fn contains_point(&self, x: S, y: S, z: S) -> bool {
468        x >= self.min_x
469            && x <= self.max_x
470            && y >= self.min_y
471            && y <= self.max_y
472            && z >= self.min_z
473            && z <= self.max_z
474    }
475
476    /// Checks if this AABB intersects another AABB.
477    pub fn intersects(&self, other: &Self) -> bool {
478        self.min_x <= other.max_x
479            && self.max_x >= other.min_x
480            && self.min_y <= other.max_y
481            && self.max_y >= other.min_y
482            && self.min_z <= other.max_z
483            && self.max_z >= other.min_z
484    }
485
486    /// Returns the intersection of two AABBs, if any.
487    pub fn intersection(&self, other: &Self) -> Option<Self> {
488        if !self.intersects(other) {
489            return None;
490        }
491
492        Some(Self {
493            min_x: self.min_x.max(other.min_x),
494            min_y: self.min_y.max(other.min_y),
495            min_z: self.min_z.max(other.min_z),
496            max_x: self.max_x.min(other.max_x),
497            max_y: self.max_y.min(other.max_y),
498            max_z: self.max_z.min(other.max_z),
499        })
500    }
501
502    /// Returns the union (bounding box) of two AABBs.
503    pub fn union(&self, other: &Self) -> Self {
504        Self {
505            min_x: self.min_x.min(other.min_x),
506            min_y: self.min_y.min(other.min_y),
507            min_z: self.min_z.min(other.min_z),
508            max_x: self.max_x.max(other.max_x),
509            max_y: self.max_y.max(other.max_y),
510            max_z: self.max_z.max(other.max_z),
511        }
512    }
513
514    /// Expands the AABB by a margin on all sides.
515    pub fn expand(&self, margin: S) -> Self {
516        Self {
517            min_x: self.min_x - margin,
518            min_y: self.min_y - margin,
519            min_z: self.min_z - margin,
520            max_x: self.max_x + margin,
521            max_y: self.max_y + margin,
522            max_z: self.max_z + margin,
523        }
524    }
525}
526
527#[cfg(test)]
528mod tests {
529    use super::*;
530    use approx::assert_relative_eq;
531    use std::f64::consts::PI;
532
533    #[test]
534    fn test_transform2d_identity() {
535        let t = Transform2D::<f64>::identity();
536        let (x, y) = t.transform_point(1.0, 2.0);
537        assert_relative_eq!(x, 1.0, epsilon = 1e-10);
538        assert_relative_eq!(y, 2.0, epsilon = 1e-10);
539    }
540
541    #[test]
542    fn test_transform2d_translation() {
543        let t = Transform2D::translation(10.0, 20.0);
544        let (x, y) = t.transform_point(1.0, 2.0);
545        assert_relative_eq!(x, 11.0, epsilon = 1e-10);
546        assert_relative_eq!(y, 22.0, epsilon = 1e-10);
547    }
548
549    #[test]
550    fn test_transform2d_rotation() {
551        let t = Transform2D::rotation(PI / 2.0);
552        let (x, y) = t.transform_point(1.0, 0.0);
553        assert_relative_eq!(x, 0.0, epsilon = 1e-10);
554        assert_relative_eq!(y, 1.0, epsilon = 1e-10);
555    }
556
557    #[test]
558    fn test_transform2d_inverse() {
559        let t = Transform2D::new(10.0, 20.0, PI / 4.0);
560        let inv = t.inverse();
561        let composed = t.then(&inv);
562        assert!(composed.is_identity(1e-10));
563    }
564
565    #[test]
566    fn test_transform3d_translation() {
567        let t = Transform3D::translation(10.0, 20.0, 30.0);
568        let (x, y, z) = t.transform_point(1.0, 2.0, 3.0);
569        assert_relative_eq!(x, 11.0, epsilon = 1e-10);
570        assert_relative_eq!(y, 22.0, epsilon = 1e-10);
571        assert_relative_eq!(z, 33.0, epsilon = 1e-10);
572    }
573
574    #[test]
575    fn test_aabb2d_from_points() {
576        let points = vec![(0.0, 0.0), (10.0, 5.0), (3.0, 8.0)];
577        let aabb = AABB2D::from_points(&points).unwrap();
578        assert_relative_eq!(aabb.min_x, 0.0);
579        assert_relative_eq!(aabb.min_y, 0.0);
580        assert_relative_eq!(aabb.max_x, 10.0);
581        assert_relative_eq!(aabb.max_y, 8.0);
582    }
583
584    #[test]
585    fn test_aabb2d_intersection() {
586        let a = AABB2D::new(0.0, 0.0, 10.0, 10.0);
587        let b = AABB2D::new(5.0, 5.0, 15.0, 15.0);
588        let intersection = a.intersection(&b).unwrap();
589        assert_relative_eq!(intersection.min_x, 5.0);
590        assert_relative_eq!(intersection.min_y, 5.0);
591        assert_relative_eq!(intersection.max_x, 10.0);
592        assert_relative_eq!(intersection.max_y, 10.0);
593    }
594
595    #[test]
596    fn test_aabb3d_volume() {
597        let aabb = AABB3D::new(0.0, 0.0, 0.0, 10.0, 20.0, 30.0);
598        assert_relative_eq!(aabb.volume(), 6000.0);
599    }
600}
u_nesting_core/transform.rs

u_nesting_core/
transform.rs
