u_nesting_core/
transform.rs

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

u_nesting_core/
transform.rs
