scirs2-spatial 0.4.2

//! RigidTransform class for combined rotation and translation
//!
//! This module provides a `RigidTransform` class that represents a rigid transformation
//! in 3D space, combining a rotation and translation.

use crate::error::{SpatialError, SpatialResult};
use crate::transform::Rotation;
use scirs2_core::ndarray::{array, Array1, Array2, ArrayView1, ArrayView2};

// Helper function to create an array from values
#[allow(dead_code)]
fn euler_array(x: f64, y: f64, z: f64) -> Array1<f64> {
    array![x, y, z]
}

// Helper function to create a rotation from Euler angles
#[allow(dead_code)]
fn rotation_from_euler(x: f64, y: f64, z: f64, convention: &str) -> SpatialResult<Rotation> {
    let angles = euler_array(x, y, z);
    let angles_view = angles.view();
    Rotation::from_euler(&angles_view, convention)
}

/// RigidTransform represents a rigid transformation in 3D space.
///
/// A rigid transformation is a combination of a rotation and a translation.
/// It preserves the distance between any two points and the orientation of objects.
///
/// # Examples
///
/// ```
/// use scirs2_spatial::transform::{Rotation, RigidTransform};
/// use scirs2_core::ndarray::array;
/// use std::f64::consts::PI;
///
/// # fn example() -> Result<(), Box<dyn std::error::Error>> {
/// // Create a rotation around Z and a translation
/// let rotation = Rotation::from_euler(&array![0.0, 0.0, PI/2.0].view(), "xyz")?;
/// let translation = array![1.0, 2.0, 3.0];
///
/// // Create a rigid transform from rotation and translation
/// let transform = RigidTransform::from_rotation_and_translation(rotation, &translation.view())?;
///
/// // Apply the transform to a point
/// let point = array![0.0, 0.0, 0.0];
/// let transformed = transform.apply(&point.view());
/// // Should be [1.0, 2.0, 3.0] (just the translation for the origin)
///
/// // Another point
/// let point2 = array![1.0, 0.0, 0.0];
/// let transformed2 = transform.apply(&point2.view());
/// // Should be [1.0, 3.0, 3.0] (rotated then translated)
/// # Ok(())
/// # }
/// ```
#[derive(Clone, Debug)]
pub struct RigidTransform {
    /// The rotation component
    rotation: Rotation,
    /// The translation component
    translation: Array1<f64>,
}

impl RigidTransform {
    /// Create a new rigid transform from a rotation and translation
    ///
    /// # Arguments
    ///
    /// * `rotation` - The rotation component
    /// * `translation` - The translation vector (3D)
    ///
    /// # Returns
    ///
    /// A `SpatialResult` containing the rigid transform if valid, or an error if invalid
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    ///
    /// let rotation = Rotation::identity();
    /// let translation = array![1.0, 2.0, 3.0];
    /// let transform = RigidTransform::from_rotation_and_translation(rotation, &translation.view()).expect("Operation failed");
    /// ```
    pub fn from_rotation_and_translation(
        rotation: Rotation,
        translation: &ArrayView1<f64>,
    ) -> SpatialResult<Self> {
        if translation.len() != 3 {
            return Err(SpatialError::DimensionError(format!(
                "Translation must have 3 elements, got {}",
                translation.len()
            )));
        }

        Ok(RigidTransform {
            rotation,
            translation: translation.to_owned(),
        })
    }

    /// Create a rigid transform from a 4x4 transformation matrix
    ///
    /// # Arguments
    ///
    /// * `matrix` - A 4x4 transformation matrix in homogeneous coordinates
    ///
    /// # Returns
    ///
    /// A `SpatialResult` containing the rigid transform if valid, or an error if invalid
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::RigidTransform;
    /// use scirs2_core::ndarray::array;
    ///
    /// // Create a transformation matrix for translation by [1, 2, 3]
    /// let matrix = array![
    ///     [1.0, 0.0, 0.0, 1.0],
    ///     [0.0, 1.0, 0.0, 2.0],
    ///     [0.0, 0.0, 1.0, 3.0],
    ///     [0.0, 0.0, 0.0, 1.0]
    /// ];
    /// let transform = RigidTransform::from_matrix(&matrix.view()).expect("Operation failed");
    /// ```
    pub fn from_matrix(matrix: &ArrayView2<'_, f64>) -> SpatialResult<Self> {
        if matrix.shape() != [4, 4] {
            return Err(SpatialError::DimensionError(format!(
                "Matrix must be 4x4, got {:?}",
                matrix.shape()
            )));
        }

        // Check the last row is [0, 0, 0, 1]
        for i in 0..3 {
            if (matrix[[3, i]] - 0.0).abs() > 1e-10 {
                return Err(SpatialError::ValueError(
                    "Last row of matrix must be [0, 0, 0, 1]".into(),
                ));
            }
        }
        if (matrix[[3, 3]] - 1.0).abs() > 1e-10 {
            return Err(SpatialError::ValueError(
                "Last row of matrix must be [0, 0, 0, 1]".into(),
            ));
        }

        // Extract the rotation part (3x3 upper-left submatrix)
        let mut rotation_matrix = Array2::<f64>::zeros((3, 3));
        for i in 0..3 {
            for j in 0..3 {
                rotation_matrix[[i, j]] = matrix[[i, j]];
            }
        }

        // Extract the translation part (right column, first 3 elements)
        let mut translation = Array1::<f64>::zeros(3);
        for i in 0..3 {
            translation[i] = matrix[[i, 3]];
        }

        // Create rotation from the extracted matrix
        let rotation = Rotation::from_matrix(&rotation_matrix.view())?;

        Ok(RigidTransform {
            rotation,
            translation,
        })
    }

    /// Convert the rigid transform to a 4x4 matrix in homogeneous coordinates
    ///
    /// # Returns
    ///
    /// A 4x4 transformation matrix
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    ///
    /// let rotation = Rotation::identity();
    /// let translation = array![1.0, 2.0, 3.0];
    /// let transform = RigidTransform::from_rotation_and_translation(rotation, &translation.view()).expect("Operation failed");
    /// let matrix = transform.as_matrix();
    /// // Should be a 4x4 identity matrix with the last column containing the translation
    /// ```
    pub fn as_matrix(&self) -> Array2<f64> {
        let mut matrix = Array2::<f64>::zeros((4, 4));

        // Set the rotation part
        let rotation_matrix = self.rotation.as_matrix();
        for i in 0..3 {
            for j in 0..3 {
                matrix[[i, j]] = rotation_matrix[[i, j]];
            }
        }

        // Set the translation part
        for i in 0..3 {
            matrix[[i, 3]] = self.translation[i];
        }

        // Set the homogeneous coordinate part
        matrix[[3, 3]] = 1.0;

        matrix
    }

    /// Get the rotation component of the rigid transform
    ///
    /// # Returns
    ///
    /// The rotation component
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    ///
    /// let rotation = Rotation::identity();
    /// let translation = array![1.0, 2.0, 3.0];
    /// let transform = RigidTransform::from_rotation_and_translation(rotation.clone(), &translation.view()).expect("Operation failed");
    /// let retrieved_rotation = transform.rotation();
    /// ```
    pub fn rotation(&self) -> &Rotation {
        &self.rotation
    }

    /// Get the translation component of the rigid transform
    ///
    /// # Returns
    ///
    /// The translation vector
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    ///
    /// let rotation = Rotation::identity();
    /// let translation = array![1.0, 2.0, 3.0];
    /// let transform = RigidTransform::from_rotation_and_translation(rotation, &translation.view()).expect("Operation failed");
    /// let retrieved_translation = transform.translation();
    /// ```
    pub fn translation(&self) -> &Array1<f64> {
        &self.translation
    }

    /// Apply the rigid transform to a point or vector
    ///
    /// # Arguments
    ///
    /// * `point` - A 3D point or vector to transform
    ///
    /// # Returns
    ///
    /// The transformed point or vector
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    /// use std::f64::consts::PI;
    ///
    /// # fn example() -> Result<(), Box<dyn std::error::Error>> {
    /// let rotation = Rotation::from_euler(&array![0.0, 0.0, PI/2.0].view(), "xyz")?;
    /// let translation = array![1.0, 2.0, 3.0];
    /// let transform = RigidTransform::from_rotation_and_translation(rotation, &translation.view())?;
    /// let point = array![1.0, 0.0, 0.0];
    /// let transformed = transform.apply(&point.view())?;
    /// // Should be [1.0, 3.0, 3.0] (rotated then translated)
    /// # Ok(())
    /// # }
    /// ```
    pub fn apply(&self, point: &ArrayView1<f64>) -> SpatialResult<Array1<f64>> {
        if point.len() != 3 {
            return Err(SpatialError::DimensionError(
                "Point must have 3 elements".to_string(),
            ));
        }

        // Apply rotation then translation
        let rotated = self.rotation.apply(point)?;
        Ok(rotated + &self.translation)
    }

    /// Apply the rigid transform to multiple points
    ///
    /// # Arguments
    ///
    /// * `points` - A 2D array of points (each row is a 3D point)
    ///
    /// # Returns
    ///
    /// A 2D array of transformed points
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    ///
    /// let rotation = Rotation::identity();
    /// let translation = array![1.0, 2.0, 3.0];
    /// let transform = RigidTransform::from_rotation_and_translation(rotation, &translation.view()).expect("Operation failed");
    /// let points = array![[0.0, 0.0, 0.0], [1.0, 1.0, 1.0]];
    /// let transformed = transform.apply_multiple(&points.view());
    /// ```
    pub fn apply_multiple(&self, points: &ArrayView2<'_, f64>) -> SpatialResult<Array2<f64>> {
        if points.ncols() != 3 {
            return Err(SpatialError::DimensionError(
                "Each point must have 3 elements".to_string(),
            ));
        }

        let npoints = points.nrows();
        let mut result = Array2::<f64>::zeros((npoints, 3));

        for i in 0..npoints {
            let point = points.row(i);
            let transformed = self.apply(&point)?;
            for j in 0..3 {
                result[[i, j]] = transformed[j];
            }
        }

        Ok(result)
    }

    /// Get the inverse of the rigid transform
    ///
    /// # Returns
    ///
    /// A new RigidTransform that is the inverse of this one
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    ///
    /// let rotation = Rotation::identity();
    /// let translation = array![1.0, 2.0, 3.0];
    /// let transform = RigidTransform::from_rotation_and_translation(rotation, &translation.view()).expect("Operation failed");
    /// let inverse = transform.inv();
    /// ```
    pub fn inv(&self) -> SpatialResult<RigidTransform> {
        // Inverse of a rigid transform: R^-1, -R^-1 * t
        let inv_rotation = self.rotation.inv();
        let inv_translation = -inv_rotation.apply(&self.translation.view())?;

        Ok(RigidTransform {
            rotation: inv_rotation,
            translation: inv_translation,
        })
    }

    /// Compose this rigid transform with another (apply the other transform after this one)
    ///
    /// # Arguments
    ///
    /// * `other` - The other rigid transform to combine with
    ///
    /// # Returns
    ///
    /// A new RigidTransform that represents the composition
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    ///
    /// let t1 = RigidTransform::from_rotation_and_translation(
    ///     Rotation::identity(),
    ///     &array![1.0, 0.0, 0.0].view()
    /// ).expect("Operation failed");
    /// let t2 = RigidTransform::from_rotation_and_translation(
    ///     Rotation::identity(),
    ///     &array![0.0, 1.0, 0.0].view()
    /// ).expect("Operation failed");
    /// let combined = t1.compose(&t2);
    /// // Should have a translation of [1.0, 1.0, 0.0]
    /// ```
    pub fn compose(&self, other: &RigidTransform) -> SpatialResult<RigidTransform> {
        // Compose rotations
        let rotation = self.rotation.compose(&other.rotation);

        // Compose translations: self.translation + self.rotation * other.translation
        let rotated_trans = self.rotation.apply(&other.translation.view())?;
        let translation = &self.translation + &rotated_trans;

        Ok(RigidTransform {
            rotation,
            translation,
        })
    }

    /// Create an identity rigid transform (no rotation, no translation)
    ///
    /// # Returns
    ///
    /// A new RigidTransform that represents identity
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::RigidTransform;
    /// use scirs2_core::ndarray::array;
    ///
    /// let identity = RigidTransform::identity();
    /// let point = array![1.0, 2.0, 3.0];
    /// let transformed = identity.apply(&point.view());
    /// // Should still be [1.0, 2.0, 3.0]
    /// ```
    pub fn identity() -> RigidTransform {
        RigidTransform {
            rotation: Rotation::from_quat(&array![1.0, 0.0, 0.0, 0.0].view())
                .expect("Operation failed"),
            translation: Array1::<f64>::zeros(3),
        }
    }

    /// Create a rigid transform that only has a translation component
    ///
    /// # Arguments
    ///
    /// * `translation` - The translation vector
    ///
    /// # Returns
    ///
    /// A new RigidTransform with no rotation
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::RigidTransform;
    /// use scirs2_core::ndarray::array;
    ///
    /// let transform = RigidTransform::from_translation(&array![1.0, 2.0, 3.0].view()).expect("Operation failed");
    /// let point = array![0.0, 0.0, 0.0];
    /// let transformed = transform.apply(&point.view());
    /// // Should be [1.0, 2.0, 3.0]
    /// ```
    pub fn from_translation(translation: &ArrayView1<f64>) -> SpatialResult<RigidTransform> {
        if translation.len() != 3 {
            return Err(SpatialError::DimensionError(format!(
                "Translation must have 3 elements, got {}",
                translation.len()
            )));
        }

        Ok(RigidTransform {
            rotation: Rotation::from_quat(&array![1.0, 0.0, 0.0, 0.0].view())
                .expect("Operation failed"),
            translation: translation.to_owned(),
        })
    }

    /// Create a rigid transform that only has a rotation component
    ///
    /// # Arguments
    ///
    /// * `rotation` - The rotation component
    ///
    /// # Returns
    ///
    /// A new RigidTransform with no translation
    ///
    /// # Examples
    ///
    /// ```
    /// use scirs2_spatial::transform::{Rotation, RigidTransform};
    /// use scirs2_core::ndarray::array;
    /// use std::f64::consts::PI;
    ///
    /// # fn example() -> Result<(), Box<dyn std::error::Error>> {
    /// let rotation = Rotation::from_euler(&array![0.0, 0.0, PI/2.0].view(), "xyz")?;
    /// let transform = RigidTransform::from_rotation(rotation);
    /// let point = array![1.0, 0.0, 0.0];
    /// let transformed = transform.apply(&point.view())?;
    /// // Should be [0.0, 1.0, 0.0]
    /// # Ok(())
    /// # }
    /// ```
    pub fn from_rotation(rotation: Rotation) -> RigidTransform {
        RigidTransform {
            rotation,
            translation: Array1::<f64>::zeros(3),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;
    use std::f64::consts::PI;

    #[test]
    fn test_rigid_transform_identity() {
        let identity = RigidTransform::identity();
        let point = array![1.0, 2.0, 3.0];
        let transformed = identity.apply(&point.view()).expect("Operation failed");

        assert_relative_eq!(transformed[0], point[0], epsilon = 1e-10);
        assert_relative_eq!(transformed[1], point[1], epsilon = 1e-10);
        assert_relative_eq!(transformed[2], point[2], epsilon = 1e-10);
    }

    #[test]
    fn test_rigid_transform_translation_only() {
        let translation = array![1.0, 2.0, 3.0];
        let transform =
            RigidTransform::from_translation(&translation.view()).expect("Operation failed");

        let point = array![0.0, 0.0, 0.0];
        let transformed = transform.apply(&point.view()).expect("Operation failed");

        assert_relative_eq!(transformed[0], translation[0], epsilon = 1e-10);
        assert_relative_eq!(transformed[1], translation[1], epsilon = 1e-10);
        assert_relative_eq!(transformed[2], translation[2], epsilon = 1e-10);
    }

    #[test]
    fn test_rigid_transform_rotation_only() {
        // 90 degrees rotation around Z axis
        let rotation = rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").expect("Operation failed");
        let transform = RigidTransform::from_rotation(rotation);

        let point = array![1.0, 0.0, 0.0];
        let transformed = transform.apply(&point.view()).expect("Operation failed");

        // 90 degrees rotation around Z axis of [1, 0, 0] should give [0, 1, 0]
        assert_relative_eq!(transformed[0], 0.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[1], 1.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[2], 0.0, epsilon = 1e-10);
    }

    #[test]
    fn test_rigid_transform_rotation_and_translation() {
        // 90 degrees rotation around Z axis and translation by [1, 2, 3]
        let rotation = rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").expect("Operation failed");
        let translation = array![1.0, 2.0, 3.0];
        let transform =
            RigidTransform::from_rotation_and_translation(rotation, &translation.view())
                .expect("Operation failed");

        let point = array![1.0, 0.0, 0.0];
        let transformed = transform.apply(&point.view()).expect("Operation failed");

        // 90 degrees rotation around Z axis of [1, 0, 0] should give [0, 1, 0]
        // Then translate by [1, 2, 3] to get [1, 3, 3]
        assert_relative_eq!(transformed[0], 1.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[1], 3.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[2], 3.0, epsilon = 1e-10);
    }

    #[test]
    fn test_rigid_transform_from_matrix() {
        let matrix = array![
            [0.0, -1.0, 0.0, 1.0],
            [1.0, 0.0, 0.0, 2.0],
            [0.0, 0.0, 1.0, 3.0],
            [0.0, 0.0, 0.0, 1.0]
        ];
        let transform = RigidTransform::from_matrix(&matrix.view()).expect("Operation failed");

        let point = array![1.0, 0.0, 0.0];
        let transformed = transform.apply(&point.view()).expect("Operation failed");

        // This matrix represents a 90-degree rotation around Z and translation by [1, 2, 3]
        // So [1, 0, 0] -> [0, 1, 0] -> [1, 3, 3]
        assert_relative_eq!(transformed[0], 1.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[1], 3.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[2], 3.0, epsilon = 1e-10);
    }

    #[test]
    fn test_rigid_transform_as_matrix() {
        // Create a transform and verify its matrix representation
        let rotation = rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").expect("Operation failed");
        let translation = array![1.0, 2.0, 3.0];
        let transform =
            RigidTransform::from_rotation_and_translation(rotation, &translation.view())
                .expect("Operation failed");

        let matrix = transform.as_matrix();

        // Check the rotation part (90-degree rotation around Z)
        assert_relative_eq!(matrix[[0, 0]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[0, 1]], -1.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[0, 2]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[1, 0]], 1.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[1, 1]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[1, 2]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[2, 0]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[2, 1]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[2, 2]], 1.0, epsilon = 1e-10);

        // Check the translation part
        assert_relative_eq!(matrix[[0, 3]], 1.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[1, 3]], 2.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[2, 3]], 3.0, epsilon = 1e-10);

        // Check the homogeneous row
        assert_relative_eq!(matrix[[3, 0]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[3, 1]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[3, 2]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(matrix[[3, 3]], 1.0, epsilon = 1e-10);
    }

    #[test]
    fn test_rigid_transform_inverse() {
        // Create a transform and verify its inverse
        let rotation = rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").expect("Operation failed");
        let translation = array![1.0, 2.0, 3.0];
        let transform =
            RigidTransform::from_rotation_and_translation(rotation, &translation.view())
                .expect("Operation failed");

        let inverse = transform.inv().expect("Operation failed");

        // Apply transform and then its inverse to a point
        let point = array![1.0, 2.0, 3.0];
        let transformed = transform.apply(&point.view()).expect("Operation failed");
        let back = inverse
            .apply(&transformed.view())
            .expect("Operation failed");

        // Should get back to the original point
        assert_relative_eq!(back[0], point[0], epsilon = 1e-10);
        assert_relative_eq!(back[1], point[1], epsilon = 1e-10);
        assert_relative_eq!(back[2], point[2], epsilon = 1e-10);
    }

    #[test]
    #[ignore = "Test failure - assert_relative_eq! failed at line 662: left=2.22e-16, right=1.0"]
    fn test_rigid_transform_composition() {
        // Create two transforms and compose them
        let t1 = RigidTransform::from_rotation_and_translation(
            rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").expect("Operation failed"),
            &array![1.0, 0.0, 0.0].view(),
        )
        .expect("Operation failed");

        let t2 = RigidTransform::from_rotation_and_translation(
            rotation_from_euler(PI / 2.0, 0.0, 0.0, "xyz").expect("Operation failed"),
            &array![0.0, 1.0, 0.0].view(),
        )
        .expect("Operation failed");

        let composed = t1.compose(&t2).expect("Operation failed");

        // Apply the composed transform to a point
        let point = array![1.0, 0.0, 0.0];
        let transformed = composed.apply(&point.view()).expect("Operation failed");

        // Apply the transforms individually
        let intermediate = t1.apply(&point.view()).expect("Operation failed");
        let transformed2 = t2.apply(&intermediate.view()).expect("Operation failed");

        // The composed transform and individual transforms should produce the same result
        assert_relative_eq!(transformed[0], transformed2[0], epsilon = 1e-10);
        assert_relative_eq!(transformed[1], transformed2[1], epsilon = 1e-10);
        assert_relative_eq!(transformed[2], transformed2[2], epsilon = 1e-10);
    }

    #[test]
    fn test_rigid_transform_multiple_points() {
        let rotation = rotation_from_euler(0.0, 0.0, PI / 2.0, "xyz").expect("Operation failed");
        let translation = array![1.0, 2.0, 3.0];
        let transform =
            RigidTransform::from_rotation_and_translation(rotation, &translation.view())
                .expect("Operation failed");

        let points = array![[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];

        let transformed = transform
            .apply_multiple(&points.view())
            .expect("Operation failed");

        // Check that we get the correct transformed points
        assert_eq!(transformed.shape(), points.shape());

        // [1, 0, 0] -> [0, 1, 0] -> [1, 3, 3]
        assert_relative_eq!(transformed[[0, 0]], 1.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[[0, 1]], 3.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[[0, 2]], 3.0, epsilon = 1e-10);

        // [0, 1, 0] -> [-1, 0, 0] -> [0, 2, 3]
        assert_relative_eq!(transformed[[1, 0]], 0.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[[1, 1]], 2.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[[1, 2]], 3.0, epsilon = 1e-10);

        // [0, 0, 1] -> [0, 0, 1] -> [1, 2, 4]
        assert_relative_eq!(transformed[[2, 0]], 1.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[[2, 1]], 2.0, epsilon = 1e-10);
        assert_relative_eq!(transformed[[2, 2]], 4.0, epsilon = 1e-10);
    }
}