psychopath/sub_crates/rmath/src/normal.rs

#![allow(dead_code)]

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

use crate::wide4::Float4;
use crate::xform::XformFull;
use crate::Vector;
use crate::{CrossProduct, DotProduct};

/// A surface normal in 3D space.
#[derive(Debug, Copy, Clone)]
#[repr(transparent)]
pub struct Normal(pub Float4);

impl Normal {
    #[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 into_vector(self) -> Vector {
        Vector(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)]
    #[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.

    pub fn xform(self, xform: &XformFull) -> Self {
        Self(self.0.vec_mul_3x3(&Float4::transpose_3x3(xform.m_inv)))
    }

    pub fn xform_inv(self, xform: &XformFull) -> Self {
        Self(self.0.vec_mul_3x3(&Float4::transpose_3x3(xform.m)))
    }

    pub fn xform_fast(self, xform: &XformFull) -> Self {
        Self(self.0.vec_mul_3x3_fast(&Float4::transpose_3x3(xform.m_inv)))
    }

    pub fn xform_inv_fast(self, xform: &XformFull) -> Self {
        Self(self.0.vec_mul_3x3_fast(&Float4::transpose_3x3(xform.m)))
    }
}

impl Add for Normal {
    type Output = Self;

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

impl Sub for Normal {
    type Output = Self;

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

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

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

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

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

impl Neg for Normal {
    type Output = Self;

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

impl PartialEq for Normal {
    #[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 Normal {
    #[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 Normal {
    #[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 = Normal::new(1.0, 2.0, 3.0);
        let v2 = Normal::new(1.5, 4.5, 2.5);
        let v3 = Normal::new(2.5, 6.5, 5.5);

        assert_eq!(v3, v1 + v2);
    }

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

        assert_eq!(v3, v1 - v2);
    }

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

        assert_eq!(v3, v1 * v2);
    }

    #[test]
    fn xform() {
        let n = Normal::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).into_full();

        assert_eq!(n.xform(&m), Normal::new(-4.0625, 1.78125, -0.03125));
        assert_eq!(n.xform(&m).xform_inv(&m), n);
    }

    #[test]
    fn xform_fast() {
        let n = Normal::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).into_full();

        assert_eq!(n.xform_fast(&m), Normal::new(-4.0625, 1.78125, -0.03125));
        assert_eq!(n.xform_fast(&m).xform_inv_fast(&m), n);
    }

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

        assert_eq!(v3, v1 / v2);
    }

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

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

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

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

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

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

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

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

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

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

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