spatialrust_math/
transform.rs

1use crate::{Mat4, Quat, Real, Vec3};
2
3/// Rigid transform represented as a 4x4 matrix.
4#[derive(Clone, Copy, Debug, PartialEq)]
5#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
6pub struct Transform3<T: Real> {
7    matrix: Mat4<T>,
8}
9
10/// Proper rigid transform: rotation + translation without scale/shear.
11#[derive(Clone, Copy, Debug, PartialEq)]
12#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
13pub struct Isometry3<T: Real> {
14    rotation: Quat<T>,
15    translation: Vec3<T>,
16}
17
18/// Trait for types that can transform 3D points.
19pub trait TransformPoint<T: Real> {
20    /// Transforms a point.
21    fn transform_point(&self, point: Vec3<T>) -> Vec3<T>;
22
23    /// Transforms a direction vector without translation.
24    fn transform_vector(&self, vector: Vec3<T>) -> Vec3<T>;
25}
26
27impl Transform3<f32> {
28    /// Creates a transform from a 4x4 matrix.
29    #[must_use]
30    pub const fn from_matrix(matrix: Mat4<f32>) -> Self {
31        Self { matrix }
32    }
33
34    /// Identity transform.
35    #[must_use]
36    pub fn identity() -> Self {
37        Self::from_matrix(Mat4::<f32>::identity())
38    }
39
40    /// Returns the underlying matrix.
41    #[must_use]
42    pub const fn matrix(&self) -> Mat4<f32> {
43        self.matrix
44    }
45}
46
47impl Transform3<f64> {
48    /// Creates a transform from a 4x4 matrix.
49    #[must_use]
50    pub const fn from_matrix(matrix: Mat4<f64>) -> Self {
51        Self { matrix }
52    }
53
54    /// Identity transform.
55    #[must_use]
56    pub fn identity() -> Self {
57        Self::from_matrix(Mat4::<f64>::identity())
58    }
59}
60
61impl TransformPoint<f32> for Transform3<f32> {
62    fn transform_point(&self, point: Vec3<f32>) -> Vec3<f32> {
63        self.matrix.transform_point(point)
64    }
65
66    fn transform_vector(&self, vector: Vec3<f32>) -> Vec3<f32> {
67        self.matrix.transform_vector(vector)
68    }
69}
70
71impl Isometry3<f32> {
72    /// Identity isometry.
73    #[must_use]
74    pub fn identity() -> Self {
75        Self::new(Quat::<f32>::identity(), Vec3::new(0.0, 0.0, 0.0))
76    }
77
78    /// Creates an isometry from rotation and translation.
79    #[must_use]
80    pub const fn new(rotation: Quat<f32>, translation: Vec3<f32>) -> Self {
81        Self { rotation, translation }
82    }
83
84    /// Returns the rotation component.
85    #[must_use]
86    pub const fn rotation(&self) -> Quat<f32> {
87        self.rotation
88    }
89
90    /// Returns the translation component.
91    #[must_use]
92    pub const fn translation(&self) -> Vec3<f32> {
93        self.translation
94    }
95
96    /// Converts the isometry to a 4x4 matrix.
97    #[must_use]
98    pub fn to_mat4(self) -> Mat4<f32> {
99        Mat4::<f32>::from_rotation_translation(self.rotation.to_mat3(), self.translation)
100    }
101
102    /// Composes two isometries.
103    #[must_use]
104    pub fn compose(self, other: Self) -> Self {
105        let rotation = self.rotation.mul(other.rotation);
106        let translation = self.rotation.to_mat3().mul_vec3(other.translation) + self.translation;
107        Self { rotation, translation }
108    }
109
110    /// Returns the inverse isometry.
111    #[must_use]
112    pub fn inverse(self) -> Self {
113        let inv_rotation =
114            Quat::new(-self.rotation.x, -self.rotation.y, -self.rotation.z, self.rotation.w)
115                .normalize();
116        let inv_translation = inv_rotation.to_mat3().mul_vec3(Vec3::new(
117            -self.translation.x,
118            -self.translation.y,
119            -self.translation.z,
120        ));
121        Self { rotation: inv_rotation, translation: inv_translation }
122    }
123}
124
125impl TransformPoint<f32> for Isometry3<f32> {
126    fn transform_point(&self, point: Vec3<f32>) -> Vec3<f32> {
127        self.rotation.to_mat3().mul_vec3(point) + self.translation
128    }
129
130    fn transform_vector(&self, vector: Vec3<f32>) -> Vec3<f32> {
131        self.rotation.to_mat3().mul_vec3(vector)
132    }
133}
134
135#[cfg(test)]
136mod tests {
137    use super::{Isometry3, TransformPoint};
138    use crate::tolerance::{approx_eq, f32_eps};
139    use crate::{Quat, Vec3};
140
141    #[test]
142    fn isometry_compose_and_inverse() {
143        let a = Isometry3::new(
144            Quat::from_axis_angle(Vec3::new(0.0, 0.0, 1.0), 0.5),
145            Vec3::new(1.0, 0.0, 0.0),
146        );
147        let b = Isometry3::new(Quat::<f32>::identity(), Vec3::new(0.0, 2.0, 0.0));
148        let composed = a.compose(b);
149        let point = Vec3::new(1.0, 1.0, 0.0);
150        let restored = composed.compose(composed.inverse()).transform_point(point);
151        assert!(approx_eq(restored.x, point.x, f32_eps()));
152        assert!(approx_eq(restored.y, point.y, f32_eps()));
153        assert!(approx_eq(restored.z, point.z, f32_eps()));
154    }
155}
spatialrust_math/transform.rs

spatialrust_math/
transform.rs
