leto 0.41.0

N-dimensional strided arrays, zero-copy views, and storage backends
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130

//! Rigid-body transforms: [`Translation3`] and [`Isometry3`].

use super::{Point3, UnitQuaternion, Vector3};
use eunomia::RealField;

/// A pure translation in 3-space.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(
    feature = "serde",
    serde(bound(deserialize = "T: serde::Deserialize<'de> + Copy + Default"))
)]
#[repr(transparent)]
pub struct Translation3<T> {
    /// Translation vector.
    pub vector: Vector3<T>,
}

impl<T> Translation3<T> {
    /// Construct from components.
    #[inline(always)]
    pub const fn new(x: T, y: T, z: T) -> Self {
        Self {
            vector: Vector3::from_array([x, y, z]),
        }
    }

    /// Wrap a translation vector.
    #[inline(always)]
    pub const fn from_vector(vector: Vector3<T>) -> Self {
        Self { vector }
    }
}

/// A direct (orientation-preserving) rigid-body transform: a rotation followed
/// by a translation, `p ↦ R·p + t`.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(
    feature = "serde",
    serde(bound(deserialize = "T: serde::Deserialize<'de> + Copy + Default"))
)]
#[repr(C)]
pub struct Isometry3<T> {
    /// Rotation component.
    pub rotation: UnitQuaternion<T>,
    /// Translation component.
    pub translation: Vector3<T>,
}

impl<T: RealField> Isometry3<T> {
    /// The identity transform.
    #[inline]
    pub fn identity() -> Self {
        Self {
            rotation: UnitQuaternion::identity(),
            translation: Vector3::zeros(),
        }
    }

    /// Construct from a translation and a rotation (rotation applied first).
    #[inline]
    pub fn from_parts(translation: Translation3<T>, rotation: UnitQuaternion<T>) -> Self {
        Self {
            rotation,
            translation: translation.vector,
        }
    }

    /// Apply to a point: `R·p + t`.
    #[inline]
    pub fn transform_point(self, p: Point3<T>) -> Point3<T> {
        Point3::from_coords(self.rotation.transform_vector(p.coords) + self.translation)
    }

    /// Apply to a vector: rotation only (vectors are translation-invariant).
    #[inline]
    pub fn transform_vector(self, v: Vector3<T>) -> Vector3<T> {
        self.rotation.transform_vector(v)
    }

    /// The inverse transform, `p ↦ Rᵀ·(p − t)`.
    #[inline]
    pub fn inverse(self) -> Self {
        let inv_rot = self.rotation.inverse();
        Self {
            rotation: inv_rot,
            translation: -inv_rot.transform_vector(self.translation),
        }
    }
}

impl<T: RealField> Default for Isometry3<T> {
    #[inline]
    fn default() -> Self {
        Self::identity()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::geometry::{Unit, Vector};

    #[test]
    fn transform_point_rotates_then_translates() {
        let z = Unit::new_normalize(Vector::from_array([0.0_f64, 0.0, 1.0]));
        let rot = UnitQuaternion::from_axis_angle(z, core::f64::consts::FRAC_PI_2);
        let iso = Isometry3::from_parts(Translation3::new(10.0, 0.0, 0.0), rot);
        // x=(1,0,0) rotates to (0,1,0), then +translation (10,0,0) → (10,1,0)
        let p = iso.transform_point(Point3::from_array([1.0, 0.0, 0.0]));
        assert!((p.coords.data[0] - 10.0).abs() < 1e-12);
        assert!((p.coords.data[1] - 1.0).abs() < 1e-12);
        assert!(p.coords.data[2].abs() < 1e-12);
    }

    #[test]
    fn inverse_round_trips() {
        let axis = Unit::new_normalize(Vector::from_array([1.0_f64, 1.0, 1.0]));
        let iso = Isometry3::from_parts(
            Translation3::new(3.0, -2.0, 5.0),
            UnitQuaternion::from_axis_angle(axis, 0.9),
        );
        let p = Point3::from_array([2.0_f64, -1.0, 4.0]);
        let round = iso.inverse().transform_point(iso.transform_point(p));
        for k in 0..3 {
            assert!((round.coords.data[k] - p.coords.data[k]).abs() < 1e-10);
        }
    }
}