Initial commit

2026-03-04 00:08:42 +00:00
commit 4d88dc0584
4449 changed files with 1556714 additions and 0 deletions
@@ -0,0 +1,286 @@
+//! Affine transformation (12 DOF).
+
+use super::Transform;
+use nalgebra::{Matrix3, Matrix4, Point3, Vector3};
+use serde::{Deserialize, Serialize};
+
+/// Affine transformation with 12 degrees of freedom.
+///
+/// Represents a 3D affine transform: y = A * x + t
+/// where A is a 3x3 matrix and t is a translation vector.
+#[derive(Debug, Clone, Serialize, Deserialize)]
+pub struct AffineTransform {
+    /// 3x3 linear transformation matrix.
+    pub matrix: Matrix3<f64>,
+    /// Translation vector.
+    pub translation: Vector3<f64>,
+}
+
+impl AffineTransform {
+    /// Create a new affine transform.
+    pub fn new(matrix: Matrix3<f64>, translation: Vector3<f64>) -> Self {
+        Self {
+            matrix,
+            translation,
+        }
+    }
+
+    /// Create an identity transform.
+    pub fn identity() -> Self {
+        Self {
+            matrix: Matrix3::identity(),
+            translation: Vector3::zeros(),
+        }
+    }
+
+    /// Create a pure translation transform.
+    pub fn translation(tx: f64, ty: f64, tz: f64) -> Self {
+        Self {
+            matrix: Matrix3::identity(),
+            translation: Vector3::new(tx, ty, tz),
+        }
+    }
+
+    /// Create a pure scaling transform.
+    pub fn scaling(sx: f64, sy: f64, sz: f64) -> Self {
+        Self {
+            matrix: Matrix3::from_diagonal(&Vector3::new(sx, sy, sz)),
+            translation: Vector3::zeros(),
+        }
+    }
+
+    /// Create a rotation around X axis.
+    pub fn rotation_x(angle: f64) -> Self {
+        let c = angle.cos();
+        let s = angle.sin();
+        Self {
+            matrix: Matrix3::new(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c),
+            translation: Vector3::zeros(),
+        }
+    }
+
+    /// Create a rotation around Y axis.
+    pub fn rotation_y(angle: f64) -> Self {
+        let c = angle.cos();
+        let s = angle.sin();
+        Self {
+            matrix: Matrix3::new(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c),
+            translation: Vector3::zeros(),
+        }
+    }
+
+    /// Create a rotation around Z axis.
+    pub fn rotation_z(angle: f64) -> Self {
+        let c = angle.cos();
+        let s = angle.sin();
+        Self {
+            matrix: Matrix3::new(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0),
+            translation: Vector3::zeros(),
+        }
+    }
+
+    /// Create a rotation from Euler angles (ZYX convention).
+    pub fn from_euler_zyx(rx: f64, ry: f64, rz: f64) -> Self {
+        let rot_x = Self::rotation_x(rx);
+        let rot_y = Self::rotation_y(ry);
+        let rot_z = Self::rotation_z(rz);
+
+        // ZYX: first Z, then Y, then X
+        
+        rot_x.compose(&rot_y).compose(&rot_z)
+    }
+
+    /// Compose two affine transforms: self followed by other.
+    pub fn compose(&self, other: &Self) -> Self {
+        Self {
+            matrix: other.matrix * self.matrix,
+            translation: other.matrix * self.translation + other.translation,
+        }
+    }
+
+    /// Get the determinant of the linear part.
+    pub fn determinant(&self) -> f64 {
+        self.matrix.determinant()
+    }
+
+    /// Check if the transform is invertible.
+    pub fn is_invertible(&self) -> bool {
+        self.determinant().abs() > 1e-10
+    }
+
+    /// Create from a 4x4 homogeneous matrix.
+    pub fn from_matrix4(m: &Matrix4<f64>) -> Self {
+        let matrix = Matrix3::new(
+            m[(0, 0)],
+            m[(0, 1)],
+            m[(0, 2)],
+            m[(1, 0)],
+            m[(1, 1)],
+            m[(1, 2)],
+            m[(2, 0)],
+            m[(2, 1)],
+            m[(2, 2)],
+        );
+        let translation = Vector3::new(m[(0, 3)], m[(1, 3)], m[(2, 3)]);
+        Self {
+            matrix,
+            translation,
+        }
+    }
+}
+
+impl Transform for AffineTransform {
+    fn transform_point(&self, point: &Point3<f64>) -> Point3<f64> {
+        Point3::from(self.matrix * point.coords + self.translation)
+    }
+
+    fn to_matrix(&self) -> Matrix4<f64> {
+        Matrix4::new(
+            self.matrix[(0, 0)],
+            self.matrix[(0, 1)],
+            self.matrix[(0, 2)],
+            self.translation.x,
+            self.matrix[(1, 0)],
+            self.matrix[(1, 1)],
+            self.matrix[(1, 2)],
+            self.translation.y,
+            self.matrix[(2, 0)],
+            self.matrix[(2, 1)],
+            self.matrix[(2, 2)],
+            self.translation.z,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+        )
+    }
+
+    fn num_parameters(&self) -> usize {
+        12 // 9 matrix elements + 3 translation
+    }
+
+    fn get_parameters(&self) -> Vec<f64> {
+        vec![
+            self.matrix[(0, 0)],
+            self.matrix[(0, 1)],
+            self.matrix[(0, 2)],
+            self.matrix[(1, 0)],
+            self.matrix[(1, 1)],
+            self.matrix[(1, 2)],
+            self.matrix[(2, 0)],
+            self.matrix[(2, 1)],
+            self.matrix[(2, 2)],
+            self.translation.x,
+            self.translation.y,
+            self.translation.z,
+        ]
+    }
+
+    fn set_parameters(&mut self, params: &[f64]) {
+        if params.len() >= 12 {
+            self.matrix[(0, 0)] = params[0];
+            self.matrix[(0, 1)] = params[1];
+            self.matrix[(0, 2)] = params[2];
+            self.matrix[(1, 0)] = params[3];
+            self.matrix[(1, 1)] = params[4];
+            self.matrix[(1, 2)] = params[5];
+            self.matrix[(2, 0)] = params[6];
+            self.matrix[(2, 1)] = params[7];
+            self.matrix[(2, 2)] = params[8];
+            self.translation.x = params[9];
+            self.translation.y = params[10];
+            self.translation.z = params[11];
+        }
+    }
+
+    fn clone_box(&self) -> Box<dyn Transform> {
+        Box::new(self.clone())
+    }
+
+    fn try_inverse(&self) -> Option<Box<dyn Transform>> {
+        if !self.is_invertible() {
+            return None;
+        }
+
+        let inv_matrix = self.matrix.try_inverse()?;
+        let inv_translation = -inv_matrix * self.translation;
+
+        Some(Box::new(Self {
+            matrix: inv_matrix,
+            translation: inv_translation,
+        }))
+    }
+}
+
+impl Default for AffineTransform {
+    fn default() -> Self {
+        Self::identity()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_identity() {
+        let t = AffineTransform::identity();
+        let p = Point3::new(1.0, 2.0, 3.0);
+        let q = t.transform_point(&p);
+        assert!((p - q).norm() < 1e-10);
+    }
+
+    #[test]
+    fn test_translation() {
+        let t = AffineTransform::translation(1.0, 2.0, 3.0);
+        let p = Point3::new(0.0, 0.0, 0.0);
+        let q = t.transform_point(&p);
+        assert!((q - Point3::new(1.0, 2.0, 3.0)).norm() < 1e-10);
+    }
+
+    #[test]
+    fn test_scaling() {
+        let t = AffineTransform::scaling(2.0, 3.0, 4.0);
+        let p = Point3::new(1.0, 1.0, 1.0);
+        let q = t.transform_point(&p);
+        assert!((q - Point3::new(2.0, 3.0, 4.0)).norm() < 1e-10);
+    }
+
+    #[test]
+    fn test_inverse() {
+        let t = AffineTransform::translation(1.0, 2.0, 3.0);
+        let inv = t.try_inverse().unwrap();
+        let p = Point3::new(1.0, 2.0, 3.0);
+        let q = t.transform_point(&p);
+        let r = inv.transform_point(&q);
+        assert!((p - r).norm() < 1e-10);
+    }
+
+    #[test]
+    fn test_compose() {
+        let t1 = AffineTransform::translation(1.0, 0.0, 0.0);
+        let t2 = AffineTransform::scaling(2.0, 2.0, 2.0);
+
+        let composed = t1.compose(&t2);
+        let p = Point3::new(1.0, 1.0, 1.0);
+
+        // First translate, then scale
+        let q1 = t1.transform_point(&p);
+        let q2 = t2.transform_point(&q1);
+
+        let q = composed.transform_point(&p);
+
+        assert!((q - q2).norm() < 1e-10);
+    }
+
+    #[test]
+    fn test_parameters() {
+        let t = AffineTransform::identity();
+        let params = t.get_parameters();
+        assert_eq!(params.len(), 12);
+
+        let mut t2 = AffineTransform::identity();
+        t2.set_parameters(&params);
+        assert!((t.matrix - t2.matrix).norm() < 1e-10);
+    }
+}