cu_spatial_payloads/

lib.rs

#![cfg_attr(not(test), no_std)]

#[cfg(feature = "rerun")]
extern crate alloc;

use bincode::{Decode, Encode};
use core::fmt::Debug;
use core::ops::Mul;
use cu29::prelude::*;
use cu29::units::si::angle::degree;
use cu29::units::si::f32::Angle as Angle32;
use cu29::units::si::f32::Length as Length32;
use cu29::units::si::f64::Angle as Angle64;
use cu29::units::si::f64::Length as Length64;
use cu29::units::si::length::meter;
use serde::{Deserialize, Serialize};

#[cfg(feature = "glam")]
use glam::{Affine3A, DAffine3, DMat4, DVec3, Mat4, Vec3, Vec3A};

mod geometry;
pub use geometry::{
    BBox, BBox2d, BBox2f, BBox2i, BBox2u, BBox3d, BBox3f, Point2, Point2Iterator, Point2Soa,
    Point2d, Point2dSoa, Point2f, Point2fSoa, Point2i, Point2iSoa, Point2u, Point2uSoa, Point3,
    Point3Iterator, Point3Soa, Point3d, Point3dSoa, Point3f, Point3fSoa,
};

#[cfg(feature = "rerun")]
mod rerun_components;

/// Geodetic position expressed in [EPSG:4326](https://epsg.io/4326) latitude and longitude.
///
/// Use this for absolute Earth-referenced positions such as GNSS fixes. Keep it distinct from
/// local engineering coordinates like ENU/NED/cartesian meters.
#[derive(
    Debug, Clone, Copy, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect,
)]
pub struct GeodeticPosition {
    pub latitude: Angle64,
    pub longitude: Angle64,
}

impl GeodeticPosition {
    pub fn new(latitude: Angle64, longitude: Angle64) -> Self {
        Self {
            latitude,
            longitude,
        }
    }

    pub fn from_degrees(latitude_deg: f64, longitude_deg: f64) -> Self {
        Self {
            latitude: Angle64::new::<degree>(latitude_deg),
            longitude: Angle64::new::<degree>(longitude_deg),
        }
    }

    pub fn latitude_degrees(&self) -> f64 {
        self.latitude.get::<degree>()
    }

    pub fn longitude_degrees(&self) -> f64 {
        self.longitude.get::<degree>()
    }
}

/// Transform3D represents a 3D transformation (rotation + translation)
/// When the glam feature is enabled, it uses glam's optimized types internally
#[derive(Debug, Clone, Copy, Reflect)]
#[reflect(opaque, from_reflect = false)]
pub struct Transform3D<T: Copy + Debug + 'static> {
    #[cfg(feature = "glam")]
    inner: TransformInner<T>,
    #[cfg(not(feature = "glam"))]
    pub mat: [[T; 4]; 4],
}

#[cfg(feature = "glam")]
#[derive(Debug, Clone, Copy)]
enum TransformInner<T: Copy + Debug + 'static> {
    F32(Affine3A),
    F64(DAffine3),
    _Phantom(core::marker::PhantomData<T>),
}

const fn const_sin_cos(mut angle: f64) -> (f64, f64) {
    const PI: f64 = core::f64::consts::PI;
    const FRAC_PI_2: f64 = core::f64::consts::FRAC_PI_2;
    const TAU: f64 = core::f64::consts::TAU;

    angle %= TAU;
    if angle > PI {
        angle -= TAU;
    } else if angle < -PI {
        angle += TAU;
    }

    let mut cos_sign = 1.0;
    if angle > FRAC_PI_2 {
        angle = PI - angle;
        cos_sign = -1.0;
    } else if angle < -FRAC_PI_2 {
        angle = -PI - angle;
        cos_sign = -1.0;
    }

    // After range reduction to [-pi/2, pi/2], Taylor terms through x^21 for sine and x^20 for
    // cosine keep the approximation below the f64 regression tolerance used by this crate.
    let x2 = angle * angle;
    let mut sin_poly = 1.0 / 51_090_942_171_709_440_000.0;
    sin_poly = -1.0 / 121_645_100_408_832_000.0 + x2 * sin_poly;
    sin_poly = 1.0 / 355_687_428_096_000.0 + x2 * sin_poly;
    sin_poly = -1.0 / 1_307_674_368_000.0 + x2 * sin_poly;
    sin_poly = 1.0 / 6_227_020_800.0 + x2 * sin_poly;
    sin_poly = -1.0 / 39_916_800.0 + x2 * sin_poly;
    sin_poly = 1.0 / 362_880.0 + x2 * sin_poly;
    sin_poly = -1.0 / 5_040.0 + x2 * sin_poly;
    sin_poly = 1.0 / 120.0 + x2 * sin_poly;
    sin_poly = -1.0 / 6.0 + x2 * sin_poly;
    let sin = angle * (1.0 + x2 * sin_poly);

    let mut cos_poly = 1.0 / 2_432_902_008_176_640_000.0;
    cos_poly = -1.0 / 6_402_373_705_728_000.0 + x2 * cos_poly;
    cos_poly = 1.0 / 20_922_789_888_000.0 + x2 * cos_poly;
    cos_poly = -1.0 / 87_178_291_200.0 + x2 * cos_poly;
    cos_poly = 1.0 / 479_001_600.0 + x2 * cos_poly;
    cos_poly = -1.0 / 3_628_800.0 + x2 * cos_poly;
    cos_poly = 1.0 / 40_320.0 + x2 * cos_poly;
    cos_poly = -1.0 / 720.0 + x2 * cos_poly;
    cos_poly = 1.0 / 24.0 + x2 * cos_poly;
    cos_poly = -1.0 / 2.0 + x2 * cos_poly;
    let cos = 1.0 + x2 * cos_poly;

    (sin, cos_sign * cos)
}

macro_rules! impl_const_transform {
    ($ty:ty, $len:ty, $ang:ty, $variant:ident, $affine:ty, $vec:ty) => {
        impl Transform3D<$ty> {
            const fn from_rows(rows: [[$ty; 4]; 3]) -> Self {
                #[cfg(feature = "glam")]
                {
                    Self {
                        inner: TransformInner::$variant(<$affine>::from_cols(
                            <$vec>::new(rows[0][0], rows[1][0], rows[2][0]),
                            <$vec>::new(rows[0][1], rows[1][1], rows[2][1]),
                            <$vec>::new(rows[0][2], rows[1][2], rows[2][2]),
                            <$vec>::new(rows[0][3], rows[1][3], rows[2][3]),
                        )),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    Self {
                        mat: [
                            rows[0],
                            rows[1],
                            rows[2],
                            [0.0 as $ty, 0.0 as $ty, 0.0 as $ty, 1.0 as $ty],
                        ],
                    }
                }
            }

            const fn rows(self) -> [[$ty; 4]; 3] {
                #[cfg(feature = "glam")]
                {
                    match self.inner {
                        TransformInner::$variant(affine) => {
                            let r = affine.matrix3;
                            let x = r.x_axis.to_array();
                            let y = r.y_axis.to_array();
                            let z = r.z_axis.to_array();
                            let t = affine.translation.to_array();
                            [
                                [x[0], y[0], z[0], t[0]],
                                [x[1], y[1], z[1], t[1]],
                                [x[2], y[2], z[2], t[2]],
                            ]
                        }
                        _ => panic!("invalid Transform3D storage variant"),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    [self.mat[0], self.mat[1], self.mat[2]]
                }
            }

            /// Creates the identity transform during const evaluation.
            pub const fn identity() -> Self {
                Self::from_rows([
                    [1.0 as $ty, 0.0 as $ty, 0.0 as $ty, 0.0 as $ty],
                    [0.0 as $ty, 1.0 as $ty, 0.0 as $ty, 0.0 as $ty],
                    [0.0 as $ty, 0.0 as $ty, 1.0 as $ty, 0.0 as $ty],
                ])
            }

            /// Creates a transform from unit-typed translation and XYZ Euler angles.
            ///
            /// `rotation` is `[roll_x, pitch_y, yaw_z]`. Rotations are applied X, then Y, then Z,
            /// producing `Rz * Ry * Rx` for column vectors. Translation is applied last.
            /// Const unit values use their SI base representation: meters and radians.
            pub const fn from_translation_euler_xyz(
                translation: [$len; 3],
                rotation: [$ang; 3],
            ) -> Self {
                let (sx, cx) = const_sin_cos(rotation[0].value as f64);
                let (sy, cy) = const_sin_cos(rotation[1].value as f64);
                let (sz, cz) = const_sin_cos(rotation[2].value as f64);
                let sx = sx as $ty;
                let cx = cx as $ty;
                let sy = sy as $ty;
                let cy = cy as $ty;
                let sz = sz as $ty;
                let cz = cz as $ty;

                Self::from_rows([
                    [
                        cy * cz,
                        cz * sx * sy - cx * sz,
                        sx * sz + cx * cz * sy,
                        translation[0].value,
                    ],
                    [
                        cy * sz,
                        cx * cz + sx * sy * sz,
                        cx * sy * sz - cz * sx,
                        translation[1].value,
                    ],
                    [-sy, cy * sx, cx * cy, translation[2].value],
                ])
            }

            /// Composes `self` with `rhs` during const evaluation.
            ///
            /// The returned transform applies `rhs` first and then `self`, matching `self * rhs`.
            pub const fn compose(self, rhs: Self) -> Self {
                let lhs = self.rows();
                let rhs = rhs.rows();
                let mut result = [[0.0 as $ty; 4]; 3];
                let mut row = 0;
                while row < 3 {
                    let mut column = 0;
                    while column < 3 {
                        result[row][column] = lhs[row][0] * rhs[0][column]
                            + lhs[row][1] * rhs[1][column]
                            + lhs[row][2] * rhs[2][column];
                        column += 1;
                    }
                    result[row][3] = lhs[row][0] * rhs[0][3]
                        + lhs[row][1] * rhs[1][3]
                        + lhs[row][2] * rhs[2][3]
                        + lhs[row][3];
                    row += 1;
                }
                Self::from_rows(result)
            }
        }
    };
}

impl_const_transform!(f32, Length32, Angle32, F32, Affine3A, Vec3A);
impl_const_transform!(f64, Length64, Angle64, F64, DAffine3, DVec3);

pub type Pose<T> = Transform3D<T>;

macro_rules! impl_transform_accessors {
    ($ty:ty, $len:ty, $variant:ident) => {
        impl Transform3D<$ty> {
            /// The translation component, in meters.
            pub fn translation(&self) -> [$len; 3] {
                let position = self.position();
                [position.x, position.y, position.z]
            }

            /// The dimensionless rotation matrix, represented as rows.
            pub fn rotation(&self) -> [[$ty; 3]; 3] {
                #[cfg(feature = "glam")]
                {
                    match &self.inner {
                        TransformInner::$variant(affine) => {
                            let r = &affine.matrix3;
                            [
                                [r.x_axis.x, r.y_axis.x, r.z_axis.x],
                                [r.x_axis.y, r.y_axis.y, r.z_axis.y],
                                [r.x_axis.z, r.y_axis.z, r.z_axis.z],
                            ]
                        }
                        _ => unreachable!(),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    [
                        [self.mat[0][0], self.mat[0][1], self.mat[0][2]],
                        [self.mat[1][0], self.mat[1][1], self.mat[1][2]],
                        [self.mat[2][0], self.mat[2][1], self.mat[2][2]],
                    ]
                }
            }
        }
    };
}

// Manual implementations for serialization
impl<T: Copy + Debug + Default + 'static> Serialize for Transform3D<T>
where
    T: Serialize,
{
    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
    where
        S: serde::Serializer,
    {
        #[cfg(feature = "glam")]
        {
            let mat = self.to_matrix();
            mat.serialize(serializer)
        }
        #[cfg(not(feature = "glam"))]
        {
            self.mat.serialize(serializer)
        }
    }
}

impl<'de, T: Copy + Debug + 'static> Deserialize<'de> for Transform3D<T>
where
    T: Deserialize<'de> + Default,
{
    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
    where
        D: serde::Deserializer<'de>,
    {
        let mat: [[T; 4]; 4] = Deserialize::deserialize(deserializer)?;
        Ok(Self::from_matrix(mat))
    }
}

// Bincode implementations
impl<T: Copy + Debug + Default + 'static> Encode for Transform3D<T>
where
    T: Encode,
{
    fn encode<E: bincode::enc::Encoder>(
        &self,
        encoder: &mut E,
    ) -> Result<(), bincode::error::EncodeError> {
        #[cfg(feature = "glam")]
        {
            let mat = self.to_matrix();
            mat.encode(encoder)
        }
        #[cfg(not(feature = "glam"))]
        {
            self.mat.encode(encoder)
        }
    }
}

impl<T: Copy + Debug + 'static> Decode<()> for Transform3D<T>
where
    T: Decode<()> + Default,
{
    fn decode<D: bincode::de::Decoder<Context = ()>>(
        decoder: &mut D,
    ) -> Result<Self, bincode::error::DecodeError> {
        let mat: [[T; 4]; 4] = Decode::decode(decoder)?;
        Ok(Self::from_matrix(mat))
    }
}

impl<T: Copy + Debug + Default + 'static> Transform3D<T> {
    /// Create a transform from a 4x4 matrix
    pub fn from_matrix(mat: [[T; 4]; 4]) -> Self {
        #[cfg(feature = "glam")]
        {
            Self {
                inner: TransformInner::from_matrix(mat),
            }
        }
        #[cfg(not(feature = "glam"))]
        {
            Self { mat }
        }
    }

    /// Get the transform as a 4x4 matrix
    pub fn to_matrix(self) -> [[T; 4]; 4] {
        #[cfg(feature = "glam")]
        {
            self.inner.to_matrix()
        }
        #[cfg(not(feature = "glam"))]
        {
            self.mat
        }
    }

    /// Get a mutable reference to the matrix (for compatibility)
    #[cfg(not(feature = "glam"))]
    pub fn mat_mut(&mut self) -> &mut [[T; 4]; 4] {
        &mut self.mat
    }
}

#[cfg(feature = "glam")]
impl<T: Copy + Debug + Default + 'static> TransformInner<T> {
    fn from_matrix(mat: [[T; 4]; 4]) -> Self {
        use core::any::TypeId;

        // This is a bit hacky but necessary for type safety
        // In practice, T will be f32 or f64
        if TypeId::of::<T>() == TypeId::of::<f32>() {
            // Convert to f32 matrix
            // SAFETY: We just verified T == f32, so the layouts match.
            let mat_f32: [[f32; 4]; 4] = unsafe { core::mem::transmute_copy(&mat) };
            let glam_mat = Mat4::from_cols_array_2d(&mat_f32);
            let affine = Affine3A::from_mat4(glam_mat);
            // SAFETY: We just verified T == f32, so this is the correct enum variant.
            unsafe { core::mem::transmute_copy(&TransformInner::<T>::F32(affine)) }
        } else if TypeId::of::<T>() == TypeId::of::<f64>() {
            // Convert to f64 matrix
            // SAFETY: We just verified T == f64, so the layouts match.
            let mat_f64: [[f64; 4]; 4] = unsafe { core::mem::transmute_copy(&mat) };
            // let m = mat_f64;
            let glam_mat = DMat4::from_cols_array_2d(&mat_f64);
            let affine = DAffine3::from_mat4(glam_mat);
            // SAFETY: We just verified T == f64, so this is the correct enum variant.
            unsafe { core::mem::transmute_copy(&TransformInner::<T>::F64(affine)) }
        } else {
            panic!("Transform3D only supports f32 and f64 types when using glam feature");
        }
    }

    fn to_matrix(self) -> [[T; 4]; 4] {
        match self {
            TransformInner::F32(affine) => {
                let mat = Mat4::from(affine);
                let mat_array = mat.to_cols_array_2d();
                // SAFETY: We only reach this arm when T == f32.
                unsafe { core::mem::transmute_copy(&mat_array) }
            }
            TransformInner::F64(affine) => {
                let mat = DMat4::from(affine);
                let mat_array = mat.to_cols_array_2d();
                // SAFETY: We only reach this arm when T == f64.
                unsafe { core::mem::transmute_copy(&mat_array) }
            }
            TransformInner::_Phantom(_) => unreachable!(),
        }
    }
}

impl_transform_accessors!(f32, Length32, F32);
impl_transform_accessors!(f64, Length64, F64);

macro_rules! impl_transform_points {
    ($ty:ty, $len:ty, $variant:ident, $vec:ty) => {
        impl Transform3D<$ty> {
            /// The translation as a point: where the pose sits in its parent
            /// frame.
            pub fn position(&self) -> Point3<$len> {
                #[cfg(feature = "glam")]
                {
                    match &self.inner {
                        TransformInner::$variant(affine) => {
                            let t = affine.translation;
                            Point3::new(
                                <$len>::new::<meter>(t.x as $ty),
                                <$len>::new::<meter>(t.y as $ty),
                                <$len>::new::<meter>(t.z as $ty),
                            )
                        }
                        _ => unreachable!(),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    Point3::new(
                        <$len>::new::<meter>(self.mat[0][3]),
                        <$len>::new::<meter>(self.mat[1][3]),
                        <$len>::new::<meter>(self.mat[2][3]),
                    )
                }
            }

            /// `p` mapped through the transform: rotation, then translation.
            pub fn transform_point(&self, p: Point3<$len>) -> Point3<$len> {
                #[cfg(feature = "glam")]
                {
                    match &self.inner {
                        TransformInner::$variant(affine) => {
                            let out = affine.transform_point3(<$vec>::new(
                                p.x.raw(),
                                p.y.raw(),
                                p.z.raw(),
                            ));
                            Point3::new(
                                <$len>::new::<meter>(out.x),
                                <$len>::new::<meter>(out.y),
                                <$len>::new::<meter>(out.z),
                            )
                        }
                        _ => unreachable!(),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    let m = &self.mat;
                    let (x, y, z) = (p.x.raw(), p.y.raw(), p.z.raw());
                    Point3::new(
                        <$len>::new::<meter>(m[0][0] * x + m[0][1] * y + m[0][2] * z + m[0][3]),
                        <$len>::new::<meter>(m[1][0] * x + m[1][1] * y + m[1][2] * z + m[1][3]),
                        <$len>::new::<meter>(m[2][0] * x + m[2][1] * y + m[2][2] * z + m[2][3]),
                    )
                }
            }

            /// `v` mapped through the rotation only, for directions and
            /// offsets.
            pub fn transform_vector(&self, v: Point3<$len>) -> Point3<$len> {
                #[cfg(feature = "glam")]
                {
                    match &self.inner {
                        TransformInner::$variant(affine) => {
                            let out = affine.transform_vector3(<$vec>::new(
                                v.x.raw(),
                                v.y.raw(),
                                v.z.raw(),
                            ));
                            Point3::new(
                                <$len>::new::<meter>(out.x),
                                <$len>::new::<meter>(out.y),
                                <$len>::new::<meter>(out.z),
                            )
                        }
                        _ => unreachable!(),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    let m = &self.mat;
                    let (x, y, z) = (v.x.raw(), v.y.raw(), v.z.raw());
                    Point3::new(
                        <$len>::new::<meter>(m[0][0] * x + m[0][1] * y + m[0][2] * z),
                        <$len>::new::<meter>(m[1][0] * x + m[1][1] * y + m[1][2] * z),
                        <$len>::new::<meter>(m[2][0] * x + m[2][1] * y + m[2][2] * z),
                    )
                }
            }

            /// Every point in the set mapped through the transform, in place.
            ///
            /// May differ from [`Self::transform_point`] by a rounding step.
            pub fn transform_points<const N: usize>(&self, points: &mut Point3Soa<$len, N>) {
                #[cfg(feature = "glam")]
                let m = match &self.inner {
                    TransformInner::$variant(affine) => {
                        let r = &affine.matrix3;
                        let t = affine.translation;
                        [
                            [r.x_axis.x, r.y_axis.x, r.z_axis.x, t.x],
                            [r.x_axis.y, r.y_axis.y, r.z_axis.y, t.y],
                            [r.x_axis.z, r.y_axis.z, r.z_axis.z, t.z],
                        ]
                    }
                    _ => unreachable!(),
                };
                #[cfg(not(feature = "glam"))]
                let m = [self.mat[0], self.mat[1], self.mat[2]];

                let n = points.len();
                for i in 0..n {
                    let (x, y, z) = (points.x[i].raw(), points.y[i].raw(), points.z[i].raw());
                    points.x[i] =
                        <$len>::new::<meter>(m[0][0] * x + m[0][1] * y + m[0][2] * z + m[0][3]);
                    points.y[i] =
                        <$len>::new::<meter>(m[1][0] * x + m[1][1] * y + m[1][2] * z + m[1][3]);
                    points.z[i] =
                        <$len>::new::<meter>(m[2][0] * x + m[2][1] * y + m[2][2] * z + m[2][3]);
                }
            }
        }
    };
}

impl_transform_points!(f32, Length32, F32, Vec3);
impl_transform_points!(f64, Length64, F64, DVec3);

impl<T: Copy + Debug + Default + 'static> Default for Transform3D<T> {
    fn default() -> Self {
        Self::from_matrix([[T::default(); 4]; 4])
    }
}

macro_rules! impl_transform_mul {
    ($ty:ty, $zero:expr, $variant:ident) => {
        impl Mul for Transform3D<$ty> {
            type Output = Self;

            fn mul(self, rhs: Self) -> Self::Output {
                #[cfg(feature = "glam")]
                {
                    match (&self.inner, &rhs.inner) {
                        (TransformInner::$variant(a), TransformInner::$variant(b)) => Self {
                            inner: TransformInner::$variant(*a * *b),
                        },
                        _ => unreachable!(),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    let mut result = [[$zero; 4]; 4];
                    for i in 0..4 {
                        for j in 0..4 {
                            let mut sum = $zero;
                            for k in 0..4 {
                                sum += self.mat[i][k] * rhs.mat[k][j];
                            }
                            result[i][j] = sum;
                        }
                    }
                    Self { mat: result }
                }
            }
        }
    };
}

impl_transform_mul!(f32, 0.0f32, F32);
impl_transform_mul!(f64, 0.0f64, F64);

/// Reference implementations for f32
impl Mul for &Transform3D<f32> {
    type Output = Transform3D<f32>;

    fn mul(self, rhs: Self) -> Self::Output {
        *self * *rhs
    }
}

impl Mul<Transform3D<f32>> for &Transform3D<f32> {
    type Output = Transform3D<f32>;

    fn mul(self, rhs: Transform3D<f32>) -> Self::Output {
        *self * rhs
    }
}

impl Mul<&Transform3D<f32>> for Transform3D<f32> {
    type Output = Transform3D<f32>;

    fn mul(self, rhs: &Transform3D<f32>) -> Self::Output {
        self * *rhs
    }
}

/// Reference implementations for f64
impl Mul for &Transform3D<f64> {
    type Output = Transform3D<f64>;

    fn mul(self, rhs: Self) -> Self::Output {
        *self * *rhs
    }
}

impl Mul<Transform3D<f64>> for &Transform3D<f64> {
    type Output = Transform3D<f64>;

    fn mul(self, rhs: Transform3D<f64>) -> Self::Output {
        *self * rhs
    }
}

impl Mul<&Transform3D<f64>> for Transform3D<f64> {
    type Output = Transform3D<f64>;

    fn mul(self, rhs: &Transform3D<f64>) -> Self::Output {
        self * *rhs
    }
}

macro_rules! impl_transform_inverse {
    ($ty:ty, $zero:expr, $one:expr, $variant:ident) => {
        impl Transform3D<$ty> {
            /// Computes the inverse of this transformation matrix.
            pub fn inverse(&self) -> Self {
                #[cfg(feature = "glam")]
                {
                    match &self.inner {
                        TransformInner::$variant(affine) => Self {
                            inner: TransformInner::$variant(affine.inverse()),
                        },
                        _ => unreachable!(),
                    }
                }
                #[cfg(not(feature = "glam"))]
                {
                    let mat = self.mat;
                    // Extract rotation matrix (top-left 3x3)
                    let r = [
                        [mat[0][0], mat[0][1], mat[0][2]],
                        [mat[1][0], mat[1][1], mat[1][2]],
                        [mat[2][0], mat[2][1], mat[2][2]],
                    ];

                    // Extract translation (top-right 3x1)
                    let t = [mat[0][3], mat[1][3], mat[2][3]];

                    // Compute transpose of rotation matrix (which is its inverse for orthogonal matrices)
                    let r_inv = [
                        [r[0][0], r[1][0], r[2][0]],
                        [r[0][1], r[1][1], r[2][1]],
                        [r[0][2], r[1][2], r[2][2]],
                    ];

                    // Compute -R^T * t
                    let t_inv = [
                        -(r_inv[0][0] * t[0] + r_inv[0][1] * t[1] + r_inv[0][2] * t[2]),
                        -(r_inv[1][0] * t[0] + r_inv[1][1] * t[1] + r_inv[1][2] * t[2]),
                        -(r_inv[2][0] * t[0] + r_inv[2][1] * t[1] + r_inv[2][2] * t[2]),
                    ];

                    // Construct the inverse transformation matrix
                    let mut inv_mat = [[$zero; 4]; 4];

                    // Copy rotation transpose
                    for i in 0..3 {
                        for j in 0..3 {
                            inv_mat[i][j] = r_inv[i][j];
                        }
                    }

                    // Copy translation part
                    inv_mat[0][3] = t_inv[0];
                    inv_mat[1][3] = t_inv[1];
                    inv_mat[2][3] = t_inv[2];

                    // Keep the homogeneous coordinate the same
                    inv_mat[3][3] = $one;

                    Self { mat: inv_mat }
                }
            }
        }
    };
}

impl_transform_inverse!(f32, 0.0f32, 1.0f32, F32);
impl_transform_inverse!(f64, 0.0f64, 1.0f64, F64);

#[cfg(feature = "faer")]
mod faer_integration {
    use super::Transform3D;
    use faer::prelude::*;

    impl From<&Transform3D<f64>> for Mat<f64> {
        fn from(p: &Transform3D<f64>) -> Self {
            let mat_array = p.to_matrix();
            let mut mat: Mat<f64> = Mat::zeros(4, 4);
            for (r, row) in mat_array.iter().enumerate() {
                for (c, item) in row.iter().enumerate() {
                    *mat.get_mut(r, c) = *item;
                }
            }
            mat
        }
    }

    impl From<Mat<f64>> for Transform3D<f64> {
        fn from(mat: Mat<f64>) -> Self {
            assert_eq!(mat.nrows(), 4);
            assert_eq!(mat.ncols(), 4);
            let mut transform = [[0.0; 4]; 4];
            for (r, row) in transform.iter_mut().enumerate() {
                for (c, val) in row.iter_mut().enumerate() {
                    *val = *mat.get(r, c);
                }
            }
            Self::from_matrix(transform)
        }
    }
}

// Optional Nalgebra integration
#[cfg(feature = "nalgebra")]
mod nalgebra_integration {
    use super::Transform3D;
    use nalgebra::{Isometry3, Matrix3, Matrix4, Rotation3, Translation3, Vector3};

    impl From<&Transform3D<f64>> for Isometry3<f64> {
        fn from(pose: &Transform3D<f64>) -> Self {
            let mat_array = pose.to_matrix();
            let flat_transform: [f64; 16] = core::array::from_fn(|i| mat_array[i / 4][i % 4]);
            let matrix = Matrix4::from_row_slice(&flat_transform);

            let rotation_matrix: Matrix3<f64> = matrix.fixed_view::<3, 3>(0, 0).into();
            let rotation = Rotation3::from_matrix_unchecked(rotation_matrix);

            let translation_vector: Vector3<f64> = matrix.fixed_view::<3, 1>(0, 3).into();
            let translation = Translation3::from(translation_vector);

            Isometry3::from_parts(translation, rotation.into())
        }
    }

    impl From<Isometry3<f64>> for Transform3D<f64> {
        fn from(iso: Isometry3<f64>) -> Self {
            let matrix = iso.to_homogeneous();
            let transform = core::array::from_fn(|r| core::array::from_fn(|c| matrix[(r, c)]));
            Transform3D::from_matrix(transform)
        }
    }
}

// Keep existing glam integration but update it
#[cfg(feature = "glam")]
mod glam_integration {
    use super::Transform3D;
    use glam::{Affine3A, DAffine3};

    impl From<Transform3D<f64>> for DAffine3 {
        fn from(p: Transform3D<f64>) -> Self {
            let mat = p.to_matrix();
            let mut aff = DAffine3::IDENTITY;
            aff.matrix3.x_axis.x = mat[0][0];
            aff.matrix3.x_axis.y = mat[0][1];
            aff.matrix3.x_axis.z = mat[0][2];

            aff.matrix3.y_axis.x = mat[1][0];
            aff.matrix3.y_axis.y = mat[1][1];
            aff.matrix3.y_axis.z = mat[1][2];

            aff.matrix3.z_axis.x = mat[2][0];
            aff.matrix3.z_axis.y = mat[2][1];
            aff.matrix3.z_axis.z = mat[2][2];

            aff.translation.x = mat[3][0];
            aff.translation.y = mat[3][1];
            aff.translation.z = mat[3][2];

            aff
        }
    }

    impl From<DAffine3> for Transform3D<f64> {
        fn from(aff: DAffine3) -> Self {
            let mut transform = [[0.0f64; 4]; 4];

            transform[0][0] = aff.matrix3.x_axis.x;
            transform[0][1] = aff.matrix3.x_axis.y;
            transform[0][2] = aff.matrix3.x_axis.z;

            transform[1][0] = aff.matrix3.y_axis.x;
            transform[1][1] = aff.matrix3.y_axis.y;
            transform[1][2] = aff.matrix3.y_axis.z;

            transform[2][0] = aff.matrix3.z_axis.x;
            transform[2][1] = aff.matrix3.z_axis.y;
            transform[2][2] = aff.matrix3.z_axis.z;

            transform[3][0] = aff.translation.x;
            transform[3][1] = aff.translation.y;
            transform[3][2] = aff.translation.z;
            transform[3][3] = 1.0;

            Transform3D::from_matrix(transform)
        }
    }

    impl From<Transform3D<f32>> for Affine3A {
        fn from(p: Transform3D<f32>) -> Self {
            let mat = p.to_matrix();
            let mut aff = Affine3A::IDENTITY;
            aff.matrix3.x_axis.x = mat[0][0];
            aff.matrix3.x_axis.y = mat[0][1];
            aff.matrix3.x_axis.z = mat[0][2];

            aff.matrix3.y_axis.x = mat[1][0];
            aff.matrix3.y_axis.y = mat[1][1];
            aff.matrix3.y_axis.z = mat[1][2];

            aff.matrix3.z_axis.x = mat[2][0];
            aff.matrix3.z_axis.y = mat[2][1];
            aff.matrix3.z_axis.z = mat[2][2];

            aff.translation.x = mat[0][3];
            aff.translation.y = mat[1][3];
            aff.translation.z = mat[2][3];

            aff
        }
    }

    impl From<Affine3A> for Transform3D<f32> {
        fn from(aff: Affine3A) -> Self {
            let mut transform = [[0.0f32; 4]; 4];

            transform[0][0] = aff.matrix3.x_axis.x;
            transform[0][1] = aff.matrix3.x_axis.y;
            transform[0][2] = aff.matrix3.x_axis.z;

            transform[1][0] = aff.matrix3.y_axis.x;
            transform[1][1] = aff.matrix3.y_axis.y;
            transform[1][2] = aff.matrix3.y_axis.z;

            transform[2][0] = aff.matrix3.z_axis.x;
            transform[2][1] = aff.matrix3.z_axis.y;
            transform[2][2] = aff.matrix3.z_axis.z;

            transform[0][3] = aff.translation.x;
            transform[1][3] = aff.translation.y;
            transform[2][3] = aff.translation.z;
            transform[3][3] = 1.0;

            Transform3D::from_matrix(transform)
        }
    }
}

#[cfg(feature = "nalgebra")]
#[allow(unused_imports)]
pub use nalgebra_integration::*;

#[cfg(feature = "faer")]
#[allow(unused_imports)]
pub use faer_integration::*;

#[cfg(test)]
mod tests {
    use super::*;

    const CONST_PARENT_TO_INTERMEDIATE: Transform3D<f32> =
        Transform3D::<f32>::from_translation_euler_xyz(
            [
                Length32 { value: 1.0 },
                Length32 { value: 2.0 },
                Length32 { value: 3.0 },
            ],
            [
                Angle32 { value: 0.0 },
                Angle32 { value: 0.0 },
                Angle32 {
                    value: core::f32::consts::FRAC_PI_2,
                },
            ],
        );
    const CONST_INTERMEDIATE_TO_CHILD: Transform3D<f32> =
        Transform3D::<f32>::from_translation_euler_xyz(
            [
                Length32 { value: 1.0 },
                Length32 { value: 0.0 },
                Length32 { value: 0.0 },
            ],
            [Angle32 { value: 0.0 }; 3],
        );
    const CONST_PARENT_TO_CHILD: Transform3D<f32> =
        CONST_PARENT_TO_INTERMEDIATE.compose(CONST_INTERMEDIATE_TO_CHILD);
    const CONST_TRANSFORM_F64: Transform3D<f64> = Transform3D::<f64>::from_translation_euler_xyz(
        [Length64 { value: 0.0 }; 3],
        [
            Angle64 {
                value: core::f64::consts::PI / 6.0,
            },
            Angle64 {
                value: -core::f64::consts::PI / 9.0,
            },
            Angle64 {
                value: core::f64::consts::PI / 18.0,
            },
        ],
    );

    fn assert_matrix_close<const N: usize, T: Copy + Into<f64>>(
        lhs: [[T; N]; N],
        rhs: [[T; N]; N],
        eps: f64,
    ) {
        for i in 0..N {
            for j in 0..N {
                let lhs = lhs[i][j].into();
                let rhs = rhs[i][j].into();
                assert!(
                    (lhs - rhs).abs() <= eps,
                    "Element at [{},{}] differs: {} vs expected {}",
                    i,
                    j,
                    lhs,
                    rhs
                );
            }
        }
    }

    #[test]
    fn const_sin_cos_matches_runtime_trigonometry() {
        for step in -64..=64 {
            let angle = f64::from(step) * core::f64::consts::PI / 8.0;
            let (actual_sin, actual_cos) = const_sin_cos(angle);
            let (expected_sin, expected_cos) = angle.sin_cos();
            assert!((actual_sin - expected_sin).abs() <= 1e-14);
            assert!((actual_cos - expected_cos).abs() <= 1e-14);
        }
    }

    #[test]
    fn const_transform_construction_and_composition_are_semantic() {
        assert_point_close(
            CONST_PARENT_TO_CHILD.position(),
            Point3f::from_meters(1.0, 3.0, 3.0),
            1e-5,
        );
        assert_point_close(
            CONST_PARENT_TO_CHILD.transform_vector(Point3f::from_meters(1.0, 0.0, 0.0)),
            Point3f::from_meters(0.0, 1.0, 0.0),
            1e-5,
        );

        let point = Point3::new(
            Length64::new::<meter>(1.0),
            Length64::new::<meter>(2.0),
            Length64::new::<meter>(3.0),
        );
        let transformed = CONST_TRANSFORM_F64.transform_vector(point);
        assert!(transformed.x.raw().is_finite());
        assert!(transformed.y.raw().is_finite());
        assert!(transformed.z.raw().is_finite());
    }

    #[test]
    fn test_pose_default() {
        let pose: Transform3D<f32> = Transform3D::default();
        let mat = pose.to_matrix();

        // With glam feature, the default is created from a zero matrix
        // but internally glam may adjust it to ensure valid transforms
        #[cfg(feature = "glam")]
        {
            // When we create from all zeros, glam will create a transform
            // that has zeros except for the homogeneous coordinate
            let expected = [
                [0.0, 0.0, 0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0],
                [0.0, 0.0, 0.0, 0.0],
                [0.0, 0.0, 0.0, 1.0], // homogeneous coordinate
            ];
            assert_eq!(mat, expected, "Default pose with glam should have w=1");
        }

        #[cfg(not(feature = "glam"))]
        {
            assert_eq!(
                mat, [[0.0; 4]; 4],
                "Default pose without glam should be a zero matrix"
            );
        }
    }

    #[test]
    fn test_transform_inverse_f32() {
        // Create a test transform with rotation and translation
        let transform = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, 2.0], // x-axis with 2m translation
            [0.0, 1.0, 0.0, 3.0], // y-axis with 3m translation
            [0.0, 0.0, 1.0, 4.0], // z-axis with 4m translation
            [0.0, 0.0, 0.0, 1.0], // homogeneous coordinate
        ]);

        // Compute inverse
        let inverse = transform.inverse();

        // Expected inverse for this transform
        let expected_inverse = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, -2.0], // Negated translation
            [0.0, 1.0, 0.0, -3.0],
            [0.0, 0.0, 1.0, -4.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Check each element with a small epsilon for floating-point comparison
        let epsilon = 1e-5;
        let inv_mat = inverse.to_matrix();
        let exp_mat = expected_inverse.to_matrix();
        assert_matrix_close(inv_mat, exp_mat, epsilon);
    }

    #[test]
    fn test_transform_inverse_f64() {
        // Create a test transform with rotation and translation
        let transform = Transform3D::<f64>::from_matrix([
            [0.0, -1.0, 0.0, 5.0], // 90-degree rotation around z with translation
            [1.0, 0.0, 0.0, 6.0],
            [0.0, 0.0, 1.0, 7.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Compute inverse
        let inverse = transform.inverse();

        // Expected inverse for this transform
        let expected_inverse = Transform3D::<f64>::from_matrix([
            [0.0, 1.0, 0.0, -6.0], // Transposed rotation and adjusted translation
            [-1.0, 0.0, 0.0, 5.0],
            [0.0, 0.0, 1.0, -7.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Check each element with a small epsilon for floating-point comparison
        let epsilon = 1e-10;
        let inv_mat = inverse.to_matrix();
        let exp_mat = expected_inverse.to_matrix();
        assert_matrix_close(inv_mat, exp_mat, epsilon);
    }

    #[test]
    fn test_transform_inverse_identity() {
        // Create identity transform
        let identity = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, 0.0],
            [0.0, 1.0, 0.0, 0.0],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Inverse of identity should be identity
        let inverse = identity.inverse();

        // Check if inverse is also identity
        let epsilon = 1e-5;
        let inv_mat = inverse.to_matrix();
        let id_mat = identity.to_matrix();
        assert_matrix_close(inv_mat, id_mat, epsilon);
    }

    #[test]
    fn test_transform_multiplication_f32() {
        // Create two transforms to multiply
        let t1 = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, 2.0], // Identity rotation + translation (2,3,4)
            [0.0, 1.0, 0.0, 3.0],
            [0.0, 0.0, 1.0, 4.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        let t2 = Transform3D::<f32>::from_matrix([
            [0.0, -1.0, 0.0, 5.0], // 90-degree rotation around z + translation (5,6,7)
            [1.0, 0.0, 0.0, 6.0],
            [0.0, 0.0, 1.0, 7.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Compute t1 * t2
        let result = t1 * t2;

        // Expected result: t1 * t2 represents first rotating by t2, then translating by t1
        let expected = Transform3D::<f32>::from_matrix([
            [0.0, -1.0, 0.0, 7.0], // Rotation from t2 + combined translation
            [1.0, 0.0, 0.0, 9.0],
            [0.0, 0.0, 1.0, 11.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Check results
        let epsilon = 1e-5;
        let res_mat = result.to_matrix();
        let exp_mat = expected.to_matrix();
        assert_matrix_close(res_mat, exp_mat, epsilon);
    }

    #[test]
    fn test_transform_multiplication_f64() {
        // Create two transforms to multiply
        let t1 = Transform3D::<f64>::from_matrix([
            [1.0, 0.0, 0.0, 2.0], // Identity rotation + translation (2,3,4)
            [0.0, 1.0, 0.0, 3.0],
            [0.0, 0.0, 1.0, 4.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        let t2 = Transform3D::<f64>::from_matrix([
            [0.0, -1.0, 0.0, 5.0], // 90-degree rotation around z + translation (5,6,7)
            [1.0, 0.0, 0.0, 6.0],
            [0.0, 0.0, 1.0, 7.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Compute t1 * t2
        let result = t1 * t2;

        // Expected result
        let expected = Transform3D::<f64>::from_matrix([
            [0.0, -1.0, 0.0, 7.0], // Rotation from t2 + combined translation
            [1.0, 0.0, 0.0, 9.0],
            [0.0, 0.0, 1.0, 11.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Check results
        let epsilon = 1e-10;
        let res_mat = result.to_matrix();
        let exp_mat = expected.to_matrix();
        assert_matrix_close(res_mat, exp_mat, epsilon);
    }

    #[test]
    fn test_transform_reference_multiplication() {
        // Test multiplication on references
        let t1 = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, 2.0],
            [0.0, 1.0, 0.0, 3.0],
            [0.0, 0.0, 1.0, 4.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        let t2 = Transform3D::<f32>::from_matrix([
            [0.0, -1.0, 0.0, 5.0],
            [1.0, 0.0, 0.0, 6.0],
            [0.0, 0.0, 1.0, 7.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Compute &t1 * &t2
        let result = t1 * t2;

        // Expected result
        let expected = Transform3D::<f32>::from_matrix([
            [0.0, -1.0, 0.0, 7.0],
            [1.0, 0.0, 0.0, 9.0],
            [0.0, 0.0, 1.0, 11.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        // Check results
        let epsilon = 1e-5;
        let res_mat = result.to_matrix();
        let exp_mat = expected.to_matrix();
        assert_matrix_close(res_mat, exp_mat, epsilon);
    }

    #[cfg(feature = "faer")]
    #[test]
    fn test_pose_faer_conversion() {
        use faer::prelude::*;

        let pose = Transform3D::from_matrix([
            [1.0, 2.0, 3.0, 4.0],
            [5.0, 6.0, 7.0, 8.0],
            [9.0, 10.0, 11.0, 12.0],
            [13.0, 14.0, 15.0, 16.0],
        ]);

        let mat: Mat<f64> = (&pose).into();
        let pose_from_mat = Transform3D::from(mat);

        assert_eq!(
            pose.to_matrix(),
            pose_from_mat.to_matrix(),
            "Faer conversion should be lossless"
        );
    }

    #[cfg(feature = "nalgebra")]
    #[test]
    fn test_pose_nalgebra_conversion() {
        use nalgebra::Isometry3;

        let pose = Transform3D::from_matrix([
            [1.0, 0.0, 0.0, 2.0],
            [0.0, 1.0, 0.0, 3.0],
            [0.0, 0.0, 1.0, 4.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);

        let iso: Isometry3<f64> = (&pose.clone()).into();
        let pose_from_iso: Transform3D<f64> = iso.into();

        assert_eq!(
            pose.to_matrix(),
            pose_from_iso.to_matrix(),
            "Nalgebra conversion should be lossless"
        );
    }

    #[cfg(feature = "glam")]
    #[test]
    fn test_pose_glam_conversion() {
        use glam::DAffine3;

        let pose = Transform3D::from_matrix([
            [1.0, 0.0, 0.0, 0.0],
            [0.0, 1.0, 0.0, 0.0],
            [0.0, 0.0, 1.0, 0.0],
            [5.0, 6.0, 7.0, 1.0],
        ]);
        let aff: DAffine3 = pose.into();
        assert_eq!(aff.translation[0], 5.0);
        let pose_from_aff: Transform3D<f64> = aff.into();

        assert_eq!(
            pose.to_matrix(),
            pose_from_aff.to_matrix(),
            "Glam conversion should be lossless"
        );
    }

    #[cfg(feature = "glam")]
    #[test]
    fn test_matrix_format_issue() {
        use glam::Mat4;

        // Test case: row-major matrix with translation in last column
        let row_major = [
            [1.0, 0.0, 0.0, 5.0], // row 0: x-axis + x translation
            [0.0, 1.0, 0.0, 6.0], // row 1: y-axis + y translation
            [0.0, 0.0, 1.0, 7.0], // row 2: z-axis + z translation
            [0.0, 0.0, 0.0, 1.0], // row 3: homogeneous
        ];

        // What glam expects: column-major format
        // Each inner array is a COLUMN, not a row
        let col_major = [
            [1.0, 0.0, 0.0, 0.0], // column 0: x-axis
            [0.0, 1.0, 0.0, 0.0], // column 1: y-axis
            [0.0, 0.0, 1.0, 0.0], // column 2: z-axis
            [5.0, 6.0, 7.0, 1.0], // column 3: translation + w
        ];

        // Create matrices
        let mat_from_row = Mat4::from_cols_array_2d(&row_major);
        let mat_from_col = Mat4::from_cols_array_2d(&col_major);

        // When using row-major data directly, translation ends up in wrong place
        assert_ne!(mat_from_row.w_axis.x, 5.0); // Translation is NOT where we expect

        // When using column-major data, translation is correct
        assert_eq!(mat_from_col.w_axis.x, 5.0);
        assert_eq!(mat_from_col.w_axis.y, 6.0);
        assert_eq!(mat_from_col.w_axis.z, 7.0);

        // The fix: transpose the row-major matrix
        let mat_transposed = Mat4::from_cols_array_2d(&row_major).transpose();
        assert_eq!(mat_transposed.w_axis.x, 5.0);
        assert_eq!(mat_transposed.w_axis.y, 6.0);
        assert_eq!(mat_transposed.w_axis.z, 7.0);
    }

    /// 90 degrees around z plus a (2, 3, 4) translation, in the layout the
    /// active backend expects: glam stores column-major, the fallback
    /// row-major.
    fn quarter_turn_and_shift() -> Transform3D<f32> {
        #[cfg(feature = "glam")]
        {
            Transform3D::from_matrix([
                [0.0, 1.0, 0.0, 0.0],
                [-1.0, 0.0, 0.0, 0.0],
                [0.0, 0.0, 1.0, 0.0],
                [2.0, 3.0, 4.0, 1.0],
            ])
        }
        #[cfg(not(feature = "glam"))]
        {
            Transform3D::from_matrix([
                [0.0, -1.0, 0.0, 2.0],
                [1.0, 0.0, 0.0, 3.0],
                [0.0, 0.0, 1.0, 4.0],
                [0.0, 0.0, 0.0, 1.0],
            ])
        }
    }

    fn assert_point_close(lhs: Point3f, rhs: Point3f, eps: f32) {
        assert!(
            (lhs.x - rhs.x).raw().abs() <= eps
                && (lhs.y - rhs.y).raw().abs() <= eps
                && (lhs.z - rhs.z).raw().abs() <= eps,
            "{lhs:?} differs from {rhs:?}"
        );
    }

    #[test]
    fn transform_point_rotates_and_translates() {
        let t = quarter_turn_and_shift();
        let p = Point3f::from_meters(1.0, 0.0, 0.0);
        assert_point_close(
            t.transform_point(p),
            Point3f::from_meters(2.0, 4.0, 4.0),
            1e-5,
        );
        assert_point_close(
            t.transform_vector(p),
            Point3f::from_meters(0.0, 1.0, 0.0),
            1e-5,
        );
        assert_point_close(t.position(), Point3f::from_meters(2.0, 3.0, 4.0), 1e-5);
    }

    #[test]
    fn transform_accessors_are_backend_independent() {
        let t = quarter_turn_and_shift();

        assert_eq!(
            t.translation(),
            [
                Length32::new::<meter>(2.0),
                Length32::new::<meter>(3.0),
                Length32::new::<meter>(4.0),
            ]
        );
        assert_eq!(
            t.rotation(),
            [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
        );
    }

    #[test]
    fn transform_point_identity_is_a_fixed_point() {
        let identity = Transform3D::<f32>::from_matrix([
            [1.0, 0.0, 0.0, 0.0],
            [0.0, 1.0, 0.0, 0.0],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ]);
        let p = Point3f::from_meters(1.5, -2.0, 0.25);
        assert_eq!(identity.transform_point(p), p);
        assert_eq!(identity.transform_vector(p), p);
        assert_eq!(identity.position(), Point3f::default());
    }

    #[test]
    fn transform_points_matches_transform_point() {
        let t = quarter_turn_and_shift();
        let points = [
            Point3f::from_meters(1.0, 0.0, 0.0),
            Point3f::from_meters(-1.5, 2.0, 0.25),
            Point3f::from_meters(0.0, 0.0, 0.0),
        ];
        let mut set = Point3fSoa::<4>::default();
        for p in points {
            set.push(p);
        }

        t.transform_points(&mut set);
        for (i, p) in points.iter().enumerate() {
            assert_point_close(set.get(i), t.transform_point(*p), 1e-5);
        }
    }

    #[test]
    fn geodetic_position_converts_to_and_from_degrees() {
        let position = GeodeticPosition::from_degrees(59.319_221, 18.075_631);

        assert_eq!(position.latitude.get::<degree>(), 59.319_221);
        assert_eq!(position.longitude.get::<degree>(), 18.075_631);
        assert_eq!(position.latitude_degrees(), 59.319_221);
        assert_eq!(position.longitude_degrees(), 18.075_631);
    }
}