psychopath/sub_crates/rmath/src/vector.rs

#![allow(dead_code)]

use std::cmp::PartialEq;
use std::ops::{Add, Div, Mul, Neg, Sub};

use crate::normal::Normal;
use crate::point::Point;
use crate::wide4::Float4;
use crate::xform::{AsXform, XformFull};
use crate::{CrossProduct, DotProduct};

/// A direction vector in 3D space.
#[derive(Debug, Copy, Clone)]
#[repr(transparent)]
pub struct Vector(pub Float4);

impl Vector {
    #[inline(always)]
    pub fn new(x: f32, y: f32, z: f32) -> Self {
        Self(Float4::new(x, y, z, 0.0))
    }

    #[inline(always)]
    pub fn length(self) -> f32 {
        self.length2().sqrt()
    }

    #[inline(always)]
    pub fn length2(self) -> f32 {
        let sqr = self.0 * self.0;
        sqr.a() + sqr.b() + sqr.c()
    }

    #[inline(always)]
    #[must_use]
    pub fn normalized(self) -> Self {
        Self(self.0 / self.length())
    }

    #[inline(always)]
    pub fn abs(self) -> Self {
        Self(self.0.abs())
    }

    #[inline(always)]
    pub fn recip(self) -> Self {
        Self(self.0.recip())
    }

    #[inline(always)]
    pub fn into_point(self) -> Point {
        Point(self.0)
    }

    #[inline(always)]
    pub fn into_normal(self) -> Normal {
        Normal(self.0)
    }

    #[inline(always)]
    pub fn x(self) -> f32 {
        self.0.a()
    }

    #[inline(always)]
    pub fn y(self) -> f32 {
        self.0.b()
    }

    #[inline(always)]
    pub fn z(self) -> f32 {
        self.0.c()
    }

    #[inline(always)]
    pub fn get_n(self, i: usize) -> f32 {
        match i {
            0 => self.x(),
            1 => self.y(),
            2 => self.z(),
            _ => panic!("Out of bounds index into 3D vector."),
        }
    }

    #[inline(always)]
    #[must_use]
    pub fn set_x(self, x: f32) -> Self {
        Self(self.0.set_a(x))
    }

    #[inline(always)]
    #[must_use]
    pub fn set_y(self, y: f32) -> Self {
        Self(self.0.set_b(y))
    }

    #[inline(always)]
    #[must_use]
    pub fn set_z(self, z: f32) -> Self {
        Self(self.0.set_c(z))
    }

    //-------------
    // Transforms.

    /// Forward-transform the vector.
    #[inline(always)]
    pub fn xform<T: AsXform>(self, xform: &T) -> Self {
        Self(self.0.vec_mul_3x3(&xform.as_xform().m))
    }

    /// Inverse-transform the vector.
    #[inline(always)]
    pub fn xform_inv(self, xform: &XformFull) -> Self {
        Self(self.0.vec_mul_3x3(&xform.inv_m))
    }

    /// Faster but less precise version of `xform()`.
    #[inline(always)]
    pub fn xform_fast<T: AsXform>(self, xform: &T) -> Self {
        Self(self.0.vec_mul_3x3_fast(&xform.as_xform().m))
    }

    /// Faster but less precise version of `xform_inv()`.
    #[inline(always)]
    pub fn xform_inv_fast(self, xform: &XformFull) -> Self {
        Self(self.0.vec_mul_3x3_fast(&xform.inv_m))
    }
}

impl Add for Vector {
    type Output = Self;

    #[inline(always)]
    fn add(self, other: Self) -> Self {
        Self(self.0 + other.0)
    }
}

impl Sub for Vector {
    type Output = Self;

    #[inline(always)]
    fn sub(self, other: Self) -> Self {
        Self(self.0 - other.0)
    }
}

impl Mul<f32> for Vector {
    type Output = Self;

    #[inline(always)]
    fn mul(self, other: f32) -> Self {
        Self(self.0 * other)
    }
}

impl Div<f32> for Vector {
    type Output = Self;

    #[inline(always)]
    fn div(self, other: f32) -> Self {
        Self(self.0 / other)
    }
}

impl Neg for Vector {
    type Output = Self;

    #[inline(always)]
    fn neg(self) -> Self {
        Self(-self.0)
    }
}

impl PartialEq for Vector {
    #[inline(always)]
    fn eq(&self, rhs: &Self) -> bool {
        self.0.a() == rhs.0.a() && self.0.b() == rhs.0.b() && self.0.c() == rhs.0.c()
    }
}

impl DotProduct for Vector {
    #[inline(always)]
    fn dot(self, other: Self) -> f32 {
        Float4::dot_3(self.0, other.0)
    }

    #[inline(always)]
    fn dot_fast(self, other: Self) -> f32 {
        Float4::dot_3_fast(self.0, other.0)
    }
}

impl CrossProduct for Vector {
    #[inline(always)]
    fn cross(self, other: Self) -> Self {
        Self(Float4::cross_3(self.0, other.0))
    }

    #[inline(always)]
    fn cross_fast(self, other: Self) -> Self {
        Self(Float4::cross_3_fast(self.0, other.0))
    }
}

//-------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{CrossProduct, DotProduct, Xform};

    #[test]
    fn add() {
        let v1 = Vector::new(1.0, 2.0, 3.0);
        let v2 = Vector::new(1.5, 4.5, 2.5);
        let v3 = Vector::new(2.5, 6.5, 5.5);

        assert_eq!(v3, v1 + v2);
    }

    #[test]
    fn sub() {
        let v1 = Vector::new(1.0, 2.0, 3.0);
        let v2 = Vector::new(1.5, 4.5, 2.5);
        let v3 = Vector::new(-0.5, -2.5, 0.5);

        assert_eq!(v3, v1 - v2);
    }

    #[test]
    fn mul_scalar() {
        let v1 = Vector::new(1.0, 2.0, 3.0);
        let v2 = 2.0;
        let v3 = Vector::new(2.0, 4.0, 6.0);

        assert_eq!(v3, v1 * v2);
    }

    #[test]
    fn xform() {
        let v = Vector::new(1.0, 2.5, 4.0);
        let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0)
            .to_full()
            .unwrap();

        assert_eq!(v.xform(&m), Vector::new(14.0, 46.0, 58.0));
        assert_eq!(v.xform(&m).xform_inv(&m), v);
    }

    #[test]
    fn xform_fast() {
        let v = Vector::new(1.0, 2.5, 4.0);
        let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0)
            .to_full()
            .unwrap();

        assert_eq!(v.xform_fast(&m), Vector::new(14.0, 46.0, 58.0));
        assert_eq!(v.xform_fast(&m).xform_inv_fast(&m), v);
    }

    #[test]
    fn div() {
        let v1 = Vector::new(1.0, 2.0, 3.0);
        let v2 = 2.0;
        let v3 = Vector::new(0.5, 1.0, 1.5);

        assert_eq!(v3, v1 / v2);
    }

    #[test]
    fn length() {
        let v = Vector::new(1.0, 2.0, 3.0);
        assert!((v.length() - 3.7416573867739413).abs() < 0.000001);
    }

    #[test]
    fn length2() {
        let v = Vector::new(1.0, 2.0, 3.0);
        assert_eq!(v.length2(), 14.0);
    }

    #[test]
    fn normalized() {
        let v1 = Vector::new(1.0, 2.0, 3.0);
        let v2 = Vector::new(0.2672612419124244, 0.5345224838248488, 0.8017837257372732);
        let v3 = v1.normalized();
        assert!((v3.x() - v2.x()).abs() < 0.000001);
        assert!((v3.y() - v2.y()).abs() < 0.000001);
        assert!((v3.z() - v2.z()).abs() < 0.000001);
    }

    #[test]
    fn dot() {
        let v1 = Vector::new(1.0, 2.0, 3.0);
        let v2 = Vector::new(1.5, 4.5, 2.5);

        assert_eq!(v1.dot(v2), 18.0);
    }

    #[test]
    fn dot_fast() {
        let v1 = Vector::new(1.0, 2.0, 3.0);
        let v2 = Vector::new(1.5, 4.5, 2.5);

        assert_eq!(v1.dot_fast(v2), 18.0);
    }

    #[test]
    fn cross() {
        let v1 = Vector::new(1.0, 0.0, 0.0);
        let v2 = Vector::new(0.0, 1.0, 0.0);

        assert_eq!(v1.cross(v2), Vector::new(0.0, 0.0, 1.0));
    }

    #[test]
    fn cross_fast() {
        let v1 = Vector::new(1.0, 0.0, 0.0);
        let v2 = Vector::new(0.0, 1.0, 0.0);

        assert_eq!(v1.cross_fast(v2), Vector::new(0.0, 0.0, 1.0));
    }
}