cumulus3d_core/

geometry.rs

/*
 * geometry.rs
 *
 * Copyright (c) 2026 ParkSangWoo
 *
 * Author(s):
 *
 *      ParkSangWoo <dev.parksangwoo@gmail.com>
 *
 *
 * This file is part of cumulus3d.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */

//! Geometry: quaternions, rigid transforms (Rigid3), similarity transforms (Sim3), angle utilities.

use nalgebra::{Matrix3, Matrix3x4, UnitQuaternion, Vector2, Vector3};
use std::ops::Mul;

/// 2D f64 vector.
pub type Vec2 = Vector2<f64>;
/// 3D f64 vector.
pub type Vec3 = Vector3<f64>;
/// 3×3 f64 matrix.
pub type Mat3 = Matrix3<f64>;
/// 3×4 f64 matrix.
pub type Mat3x4 = Matrix3x4<f64>;

/// Hamilton quaternion (i²=j²=k²=ijk=−1, active rotation v' = q v q*). Field order w, x, y, z.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Quat {
    /// Real part.
    pub w: f64,
    /// i component.
    pub x: f64,
    /// j component.
    pub y: f64,
    /// k component.
    pub z: f64,
}

impl Default for Quat {
    fn default() -> Self {
        Self::IDENTITY
    }
}

impl Quat {
    /// Identity rotation.
    pub const IDENTITY: Quat = Quat { w: 1.0, x: 0.0, y: 0.0, z: 0.0 };

    /// Creates from (w, x, y, z) (not normalized).
    pub fn new(w: f64, x: f64, y: f64, z: f64) -> Self {
        Self { w, x, y, z }
    }
    /// From file order [w, x, y, z].
    pub fn from_wxyz(v: [f64; 4]) -> Self {
        Self { w: v[0], x: v[1], y: v[2], z: v[3] }
    }
    /// `[w, x, y, z]` array.
    pub fn to_wxyz(&self) -> [f64; 4] {
        [self.w, self.x, self.y, self.z]
    }
    /// Norm.
    pub fn norm(&self) -> f64 {
        (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
    }
    /// Normalizes. Identity if the norm is 0.
    pub fn normalized(&self) -> Self {
        let n = self.norm();
        if n == 0.0 || !n.is_finite() {
            return Self::IDENTITY;
        }
        Self { w: self.w / n, x: self.x / n, y: self.y / n, z: self.z / n }
    }
    /// Conjugate.
    pub fn conjugate(&self) -> Self {
        Self { w: self.w, x: -self.x, y: -self.y, z: -self.z }
    }
    /// Inverse of a unit quaternion = conjugate.
    pub fn inverse(&self) -> Self {
        self.conjugate()
    }
    /// 4D dot product.
    pub fn dot(&self, o: &Quat) -> f64 {
        self.w * o.w + self.x * o.x + self.y * o.y + self.z * o.z
    }
    /// Hamilton product self ⊗ o (rotates by o first, then self).
    pub fn hamilton(&self, o: &Quat) -> Quat {
        Quat {
            w: self.w * o.w - self.x * o.x - self.y * o.y - self.z * o.z,
            x: self.w * o.x + self.x * o.w + self.y * o.z - self.z * o.y,
            y: self.w * o.y - self.x * o.z + self.y * o.w + self.z * o.x,
            z: self.w * o.z + self.x * o.y - self.y * o.x + self.z * o.w,
        }
    }
    /// Rotation matrix. Computed as a normalized rotation even if not unit.
    pub fn to_rotation_matrix(&self) -> Mat3 {
        let n2 = self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z;
        let s = if n2 > 0.0 { 2.0 / n2 } else { 0.0 };
        let (w, x, y, z) = (self.w, self.x, self.y, self.z);
        let (xx, yy, zz) = (x * x * s, y * y * s, z * z * s);
        let (xy, xz, yz) = (x * y * s, x * z * s, y * z * s);
        let (wx, wy, wz) = (w * x * s, w * y * s, w * z * s);
        Mat3::new(1.0 - yy - zz, xy - wz, xz + wy, xy + wz, 1.0 - xx - zz, yz - wx, xz - wy, yz + wx, 1.0 - xx - yy)
    }
    /// Rotation matrix -> unit quaternion (Shepperd's method).
    pub fn from_rotation_matrix(m: &Mat3) -> Quat {
        let tr = m[(0, 0)] + m[(1, 1)] + m[(2, 2)];
        let q = if tr > 0.0 {
            let s = (tr + 1.0).sqrt() * 2.0;
            Quat { w: 0.25 * s, x: (m[(2, 1)] - m[(1, 2)]) / s, y: (m[(0, 2)] - m[(2, 0)]) / s, z: (m[(1, 0)] - m[(0, 1)]) / s }
        } else if m[(0, 0)] > m[(1, 1)] && m[(0, 0)] > m[(2, 2)] {
            let s = (1.0 + m[(0, 0)] - m[(1, 1)] - m[(2, 2)]).sqrt() * 2.0;
            Quat { w: (m[(2, 1)] - m[(1, 2)]) / s, x: 0.25 * s, y: (m[(0, 1)] + m[(1, 0)]) / s, z: (m[(0, 2)] + m[(2, 0)]) / s }
        } else if m[(1, 1)] > m[(2, 2)] {
            let s = (1.0 + m[(1, 1)] - m[(0, 0)] - m[(2, 2)]).sqrt() * 2.0;
            Quat { w: (m[(0, 2)] - m[(2, 0)]) / s, x: (m[(0, 1)] + m[(1, 0)]) / s, y: 0.25 * s, z: (m[(1, 2)] + m[(2, 1)]) / s }
        } else {
            let s = (1.0 + m[(2, 2)] - m[(0, 0)] - m[(1, 1)]).sqrt() * 2.0;
            Quat { w: (m[(1, 0)] - m[(0, 1)]) / s, x: (m[(0, 2)] + m[(2, 0)]) / s, y: (m[(1, 2)] + m[(2, 1)]) / s, z: 0.25 * s }
        };
        q.normalized()
    }
    /// Axis-angle (radians). The axis is normalized.
    pub fn from_axis_angle(axis: &Vec3, angle: f64) -> Quat {
        let n = axis.norm();
        if n == 0.0 {
            return Self::IDENTITY;
        }
        let a = axis / n;
        let (s, c) = (0.5 * angle).sin_cos();
        Quat { w: c, x: a.x * s, y: a.y * s, z: a.z * s }
    }
    /// Rotation vector (exponential map) -> quaternion.
    pub fn from_rotation_vector(rv: &Vec3) -> Quat {
        let theta = rv.norm();
        if theta < 1e-12 {
            // First-order approximation (keeps accuracy at small angles).
            return Quat { w: 1.0, x: 0.5 * rv.x, y: 0.5 * rv.y, z: 0.5 * rv.z }.normalized();
        }
        Self::from_axis_angle(rv, theta)
    }
    /// Quaternion -> rotation vector (logarithm map), angle ∈ [0, π].
    pub fn to_rotation_vector(&self) -> Vec3 {
        let q = self.normalized();
        let q = if q.w < 0.0 { Quat { w: -q.w, x: -q.x, y: -q.y, z: -q.z } } else { q };
        let v = Vec3::new(q.x, q.y, q.z);
        let s = v.norm();
        if s < 1e-12 {
            return 2.0 * v;
        }
        let angle = 2.0 * s.atan2(q.w);
        v * (angle / s)
    }
    /// Rotates a vector.
    pub fn rotate(&self, v: &Vec3) -> Vec3 {
        self.to_rotation_matrix() * v
    }
    /// Angular distance between two rotations (radians) = 2·acos|⟨q1,q2⟩|.
    pub fn angular_distance(&self, other: &Quat) -> f64 {
        let d = self.normalized().dot(&other.normalized()).abs().min(1.0);
        2.0 * d.acos()
    }
    /// Converts to a nalgebra unit quaternion.
    pub fn to_nalgebra(&self) -> UnitQuaternion<f64> {
        UnitQuaternion::from_quaternion(nalgebra::Quaternion::new(self.w, self.x, self.y, self.z))
    }
    /// Converts from a nalgebra unit quaternion.
    pub fn from_nalgebra(q: &UnitQuaternion<f64>) -> Quat {
        Quat { w: q.w, x: q.i, y: q.j, z: q.k }
    }
}

impl Mul for Quat {
    type Output = Quat;
    fn mul(self, rhs: Quat) -> Quat {
        self.hamilton(&rhs)
    }
}

/// Skew-symmetric (cross-product) matrix `[v]×`.
pub fn skew(v: &Vec3) -> Mat3 {
    Mat3::new(0.0, -v.z, v.y, v.z, 0.0, -v.x, -v.y, v.x, 0.0)
}

/// Rigid transform A_to_B: X_B = R·X_A + t.
#[derive(Clone, Copy, Debug, PartialEq, Default)]
pub struct Rigid3 {
    /// Rotation R.
    pub rotation: Quat,
    /// Translation t.
    pub translation: Vec3,
}

impl Rigid3 {
    /// Identity transform.
    pub fn identity() -> Self {
        Self { rotation: Quat::IDENTITY, translation: Vec3::zeros() }
    }
    /// Creates from (R, t).
    pub fn new(rotation: Quat, translation: Vec3) -> Self {
        Self { rotation, translation }
    }
    /// Creates from a rotation matrix and a translation.
    pub fn from_rotation_matrix(r: &Mat3, t: Vec3) -> Self {
        Self { rotation: Quat::from_rotation_matrix(r), translation: t }
    }
    /// File order [qw, qx, qy, qz, tx, ty, tz].
    pub fn from_params(p: &[f64; 7]) -> Self {
        Self { rotation: Quat::new(p[0], p[1], p[2], p[3]), translation: Vec3::new(p[4], p[5], p[6]) }
    }
    /// File order `[qw, qx, qy, qz, tx, ty, tz]`.
    pub fn to_params(&self) -> [f64; 7] {
        let q = self.rotation;
        let t = self.translation;
        [q.w, q.x, q.y, q.z, t.x, t.y, t.z]
    }
    /// Rotation matrix R.
    pub fn rotation_matrix(&self) -> Mat3 {
        self.rotation.to_rotation_matrix()
    }
    /// [R | t].
    pub fn matrix(&self) -> Mat3x4 {
        let r = self.rotation_matrix();
        let mut m = Mat3x4::zeros();
        m.fixed_view_mut::<3, 3>(0, 0).copy_from(&r);
        m.set_column(3, &self.translation);
        m
    }
    /// Creates from a `[R | t]` matrix.
    pub fn from_matrix(m: &Mat3x4) -> Self {
        let r: Mat3 = m.fixed_view::<3, 3>(0, 0).into_owned();
        Self::from_rotation_matrix(&r, m.column(3).into_owned())
    }
    /// X_B = R X_A + t.
    pub fn transform_point(&self, p: &Vec3) -> Vec3 {
        self.rotation_matrix() * p + self.translation
    }
    /// Inverse transform B_to_A: R' = Rᵀ, t' = −Rᵀ t.
    pub fn inverse(&self) -> Self {
        let rinv = self.rotation.conjugate();
        let rt = rinv.to_rotation_matrix();
        Self { rotation: rinv, translation: -(rt * self.translation) }
    }
    /// Composition A_to_C = self(B_to_C) ∘ rhs(A_to_B). The resulting rotation is normalized.
    pub fn compose(&self, rhs: &Rigid3) -> Rigid3 {
        Rigid3 {
            rotation: self.rotation.hamilton(&rhs.rotation).normalized(),
            translation: self.translation + self.rotation_matrix() * rhs.translation,
        }
    }
    /// For world_to_cam, the projection center C = −Rᵀ t.
    pub fn center(&self) -> Vec3 {
        -(self.rotation_matrix().transpose() * self.translation)
    }
    /// For world_to_cam, the viewing direction (camera +z in world coordinates) = third row of R.
    pub fn viewing_direction(&self) -> Vec3 {
        let r = self.rotation_matrix();
        Vec3::new(r[(2, 0)], r[(2, 1)], r[(2, 2)])
    }
}

impl Mul for Rigid3 {
    type Output = Rigid3;
    fn mul(self, rhs: Rigid3) -> Rigid3 {
        self.compose(&rhs)
    }
}

impl Mul<Vec3> for Rigid3 {
    type Output = Vec3;
    fn mul(self, rhs: Vec3) -> Vec3 {
        self.transform_point(&rhs)
    }
}

/// Similarity transform new_from_old: X_new = s·R·X_old + t.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Sim3 {
    /// Scale s.
    pub scale: f64,
    /// Rotation R.
    pub rotation: Quat,
    /// Translation t.
    pub translation: Vec3,
}

impl Default for Sim3 {
    fn default() -> Self {
        Self::identity()
    }
}

impl Sim3 {
    /// Identity transform.
    pub fn identity() -> Self {
        Self { scale: 1.0, rotation: Quat::IDENTITY, translation: Vec3::zeros() }
    }
    /// Creates from (s, R, t).
    pub fn new(scale: f64, rotation: Quat, translation: Vec3) -> Self {
        Self { scale, rotation, translation }
    }
    /// [sR | t].
    pub fn matrix(&self) -> Mat3x4 {
        let r = self.rotation.to_rotation_matrix() * self.scale;
        let mut m = Mat3x4::zeros();
        m.fixed_view_mut::<3, 3>(0, 0).copy_from(&r);
        m.set_column(3, &self.translation);
        m
    }
    /// [sR | t] -> Sim3. s = norm of the first column, R = upper-left 3×3 / s.
    pub fn from_matrix(m: &Mat3x4) -> Self {
        let sr: Mat3 = m.fixed_view::<3, 3>(0, 0).into_owned();
        let s = sr.column(0).norm();
        Self { scale: s, rotation: Quat::from_rotation_matrix(&(sr / s)), translation: m.column(3).into_owned() }
    }
    /// X_new = s·R·X_old + t.
    pub fn transform_point(&self, p: &Vec3) -> Vec3 {
        self.scale * (self.rotation.to_rotation_matrix() * p) + self.translation
    }
    /// Inverse transform: s' = 1/s, R' = Rᵀ, t' = −Rᵀ t / s.
    pub fn inverse(&self) -> Self {
        let rinv = self.rotation.conjugate();
        Self { scale: 1.0 / self.scale, rotation: rinv, translation: -(rinv.to_rotation_matrix() * self.translation) / self.scale }
    }
    /// Composition self ∘ rhs.
    pub fn compose(&self, rhs: &Sim3) -> Sim3 {
        Sim3 {
            scale: self.scale * rhs.scale,
            rotation: self.rotation.hamilton(&rhs.rotation).normalized(),
            translation: self.scale * (self.rotation.to_rotation_matrix() * rhs.translation) + self.translation,
        }
    }
    /// Applies to a pose (world_to_cam or world_to_rig): R' = R Qᵀ, t' = s t − R' T.
    pub fn transform_pose(&self, world_to_cam: &Rigid3) -> Rigid3 {
        let rot = world_to_cam.rotation.hamilton(&self.rotation.conjugate()).normalized();
        let t = self.scale * world_to_cam.translation - rot.to_rotation_matrix() * self.translation;
        Rigid3 { rotation: rot, translation: t }
    }
}

impl Mul for Sim3 {
    type Output = Sim3;
    fn mul(self, rhs: Sim3) -> Sim3 {
        self.compose(&rhs)
    }
}

/// Degrees -> radians.
pub fn deg_to_rad(d: f64) -> f64 {
    d.to_radians()
}
/// Radians -> degrees.
pub fn rad_to_deg(r: f64) -> f64 {
    r.to_degrees()
}

/// Angle between two vectors (radians), acos(clamp(dot/norm product)). 0 if a norm is 0.
pub fn angle_between(a: &Vec3, b: &Vec3) -> f64 {
    let n = a.norm() * b.norm();
    if n == 0.0 {
        return 0.0;
    }
    (a.dot(b) / n).clamp(-1.0, 1.0).acos()
}

/// Triangulation angle: min(θ, π−θ) of the angle θ between (X−C1) and (X−C2).
pub fn triangulation_angle(center1: &Vec3, center2: &Vec3, point: &Vec3) -> f64 {
    let theta = angle_between(&(point - center1), &(point - center2));
    theta.min(std::f64::consts::PI - theta)
}

/// Baseline length ‖C1 − C2‖.
pub fn baseline(center1: &Vec3, center2: &Vec3) -> f64 {
    (center1 - center2).norm()
}

/// Maximum triangulation angle (radians) over several projection-center pairs. 0 if fewer than 2 centers.
pub fn max_triangulation_angle(centers: &[Vec3], point: &Vec3) -> f64 {
    let mut best = 0.0f64;
    for i in 0..centers.len() {
        for j in 0..i {
            best = best.max(triangulation_angle(&centers[i], &centers[j], point));
        }
    }
    best
}

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

    fn rand_quat(seed: u64) -> Quat {
        let f = |k: u64| (((seed * 7919 + k * 104729) % 1000) as f64 / 500.0) - 1.0;
        Quat::new(f(1), f(2), f(3), f(4) + 0.1).normalized()
    }

    #[test]
    fn quat_matrix_roundtrip() {
        for s in 0..50 {
            let q = rand_quat(s);
            let m = q.to_rotation_matrix();
            assert!((m * m.transpose() - Mat3::identity()).norm() < 1e-12);
            assert!((m.determinant() - 1.0).abs() < 1e-12);
            let q2 = Quat::from_rotation_matrix(&m);
            assert!(q.angular_distance(&q2) < 1e-7);
            assert!((q2.to_rotation_matrix() - m).norm() < 1e-12);
        }
    }

    #[test]
    fn hamilton_convention() {
        // i*j = k
        let i = Quat::new(0.0, 1.0, 0.0, 0.0);
        let j = Quat::new(0.0, 0.0, 1.0, 0.0);
        assert_eq!(i * j, Quat::new(0.0, 0.0, 0.0, 1.0));
        // 90° about z maps x to y (active rotation)
        let q = Quat::from_axis_angle(&Vec3::z(), std::f64::consts::FRAC_PI_2);
        assert!((q.rotate(&Vec3::x()) - Vec3::y()).norm() < 1e-15);
        // matrix product matches quaternion product
        let a = rand_quat(3);
        let b = rand_quat(4);
        let m = (a * b).to_rotation_matrix();
        assert!((m - a.to_rotation_matrix() * b.to_rotation_matrix()).norm() < 1e-12);
        // nalgebra agrees
        let na = a.to_nalgebra();
        assert!((na.to_rotation_matrix().matrix() - a.to_rotation_matrix()).norm() < 1e-12);
    }

    #[test]
    fn rotation_vector_roundtrip() {
        for s in 0..20 {
            let q = rand_quat(s);
            let rv = q.to_rotation_vector();
            let q2 = Quat::from_rotation_vector(&rv);
            assert!(q.angular_distance(&q2) < 1e-7);
        }
    }

    #[test]
    fn rigid3_identities() {
        let a = Rigid3::new(rand_quat(1), Vec3::new(1.0, -2.0, 3.0));
        let b = Rigid3::new(rand_quat(2), Vec3::new(-0.5, 0.2, 4.0));
        let p = Vec3::new(0.3, 0.7, -1.1);
        let ab = a * b;
        assert!(((ab * p) - (a * (b * p))).norm() < 1e-12);
        let ia = a.inverse();
        assert!(((ia * (a * p)) - p).norm() < 1e-12);
        let id = a * ia;
        assert!(id.translation.norm() < 1e-12);
        assert!(id.rotation.angular_distance(&Quat::IDENTITY) < 1e-7);
        // center maps to camera origin
        assert!((a * a.center()).norm() < 1e-12);
        let vd = a.viewing_direction();
        assert!((a.rotation_matrix() * vd - Vec3::z()).norm() < 1e-12);
        let m = Rigid3::from_matrix(&a.matrix());
        assert!(((m * p) - (a * p)).norm() < 1e-12);
    }

    #[test]
    fn sim3_identities() {
        let s = Sim3::new(2.5, rand_quat(5), Vec3::new(1.0, 2.0, 3.0));
        let t = Sim3::new(0.3, rand_quat(6), Vec3::new(-1.0, 0.0, 0.5));
        let p = Vec3::new(0.1, -0.2, 0.3);
        assert!(((s * t).transform_point(&p) - s.transform_point(&t.transform_point(&p))).norm() < 1e-12);
        assert!((s.inverse().transform_point(&s.transform_point(&p)) - p).norm() < 1e-12);
        let m = Sim3::from_matrix(&s.matrix());
        assert!((m.scale - s.scale).abs() < 1e-12);
        assert!((m.transform_point(&p) - s.transform_point(&p)).norm() < 1e-12);
        // pose transform: center moves like a point, projections unchanged up to scale
        let pose = Rigid3::new(rand_quat(7), Vec3::new(0.2, 0.1, 5.0));
        let np = s.transform_pose(&pose);
        assert!((np.center() - s.transform_point(&pose.center())).norm() < 1e-10);
        let x = Vec3::new(0.5, 0.5, 2.0);
        let c_old = pose * x;
        let c_new = np * s.transform_point(&x);
        assert!((c_new - s.scale * c_old).norm() < 1e-10);
    }

    #[test]
    fn tri_angle_symmetric() {
        let c1 = Vec3::new(-1.0, 0.0, 0.0);
        let c2 = Vec3::new(1.0, 0.0, 0.0);
        let x = Vec3::new(0.0, 0.0, 1.0);
        assert!((triangulation_angle(&c1, &c2, &x) - std::f64::consts::FRAC_PI_2).abs() < 1e-12);
        let x = Vec3::new(0.0, 0.0, 3.0_f64.sqrt());
        assert!((triangulation_angle(&c1, &c2, &x) - std::f64::consts::FRAC_PI_3).abs() < 1e-12);
        // obtuse folded: angle 120° → 60°
        let x = Vec3::new(0.0, 0.0, 1.0 / 3.0_f64.sqrt());
        assert!((triangulation_angle(&c1, &c2, &x) - std::f64::consts::FRAC_PI_3).abs() < 1e-12);
        assert!(triangulation_angle(&c1, &c1, &x) < 1e-7);
        assert_eq!(triangulation_angle(&c1, &c2, &c1), 0.0);
    }
}