bb_geometry/linear_algebra/vector3/

mod.rs

use super::matrix3x3::*;
use crate::linear_algebra::EPSILON;
use std::ops::{Add, Index, IndexMut, Mul, Neg, Sub};

/// Represents a 3-dimensional vector with f32 components.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Vector3 {
    /// The vector data stored as an array.
    data: [f32; 3],
}

impl Vector3 {
    /// Creates a new 3-vector from the provided array data.
    ///
    /// # Example
    /// ```
    /// use bb_geometry::linear_algebra::vector3::*;
    /// let v = Vector3::new([1.0, 2.0, 3.0]);
    /// assert_eq!(v.get(0), 1.0);
    /// ```
    pub fn new(data: [f32; 3]) -> Self {
        Self { data }
    }

    /// Creates a zero vector (all components are 0.0).
    ///
    /// # Example
    /// ```
    /// use bb_geometry::linear_algebra::vector3::*;
    /// let zero = Vector3::zero();
    /// assert_eq!(zero.get(1), 0.0);
    /// ```
    pub fn zero() -> Self {
        Self { data: [0.0; 3] }
    }

    /// Creates a unit vector along the X axis ([1.0, 0.0, 0.0]).
    pub fn unit_x() -> Self {
        Self {
            data: [1.0, 0.0, 0.0],
        }
    }

    /// Creates a unit vector along the Y axis ([0.0, 1.0, 0.0]).
    pub fn unit_y() -> Self {
        Self {
            data: [0.0, 1.0, 0.0],
        }
    }

    /// Creates a unit vector along the Z axis ([0.0, 0.0, 1.0]).
    pub fn unit_z() -> Self {
        Self {
            data: [0.0, 0.0, 1.0],
        }
    }

    /// Gets the value at the specified index using a method call.
    ///
    /// Prefer using the index operator `vector[index]` for more idiomatic access.
    ///
    /// # Panics
    /// Panics if `index` is out of bounds (>= 3).
    ///
    /// # Example
    /// ```
    /// use bb_geometry::linear_algebra::vector3::*;
    /// let v = Vector3::new([1.0, 2.0, 3.0]);
    /// assert_eq!(v.get(1), 2.0);
    /// ```
    #[inline]
    pub fn get(&self, index: usize) -> f32 {
        assert!(index < 3, "Index out of bounds: {}", index);
        self.data[index]
    }

    /// Sets the value at the specified index using a method call.
    ///
    /// Prefer using the mutable index operator `vector[index] = value`
    /// for more idiomatic modification.
    ///
    /// # Panics
    /// Panics if `index` is out of bounds (>= 3).
    ///
    /// # Example
    /// ```
    /// use bb_geometry::linear_algebra::vector3::*;
    /// let mut v = Vector3::zero();
    /// v.set(2, 5.0);
    /// assert_eq!(v.get(2), 5.0);
    /// ```
    #[inline]
    pub fn set(&mut self, index: usize, value: f32) {
        assert!(index < 3, "Index out of bounds: {}", index);
        self.data[index] = value;
    }

    /// Calculates the dot (inner) product of this vector with another vector.
    ///
    /// result = self[0]*other[0] + self[1]*other[1] + self[2]*other[2]
    ///
    /// # Example
    /// ```
    /// use bb_geometry::linear_algebra::vector3::*;
    /// let v1 = Vector3::new([1.0, 2.0, 3.0]);
    /// let v2 = Vector3::new([4.0, 5.0, 6.0]);
    /// // 1*4 + 2*5 + 3*6 = 4 + 10 + 18 = 32
    /// assert_eq!(v1.dot(&v2), 32.0);
    /// ```
    pub fn dot(&self, other: &Self) -> f32 {
        let mut sum = 0.0;
        for i in 0..3 {
            sum += self.data[i] * other.data[i];
        }
        sum
    }

    pub fn l2_norm(&self) -> f32 {
        let mut sum = 0.0;
        for i in 0..3 {
            sum += self.data[i] * self.data[i];
        }
        sum.sqrt()
    }

    pub fn l1_norm(&self) -> f32 {
        let mut sum = 0.0;
        for i in 0..3 {
            let inc = self.data[i].abs();
            if inc > sum {
                sum = inc;
            }
        }
        sum
    }

    /// Is true if none component of the vector exceeds EPSILON
    ///
    /// # Example
    /// ```
    /// use crate::bb_geometry::linear_algebra::vector3::*;
    /// use crate::bb_geometry::linear_algebra::EPSILON;
    ///
    /// let zero_vector = Vector3::zero();
    /// let vector_with_epsilon_entries = Vector3::new([EPSILON, EPSILON, 0.0]);
    /// let vector_with_entries_exceeding_epsilon = Vector3::new([EPSILON, 1.1 * EPSILON, 0.0]);
    ///
    /// let zero_vector_is_almost_zero = zero_vector.is_almost_zero();
    /// let vector_with_epsilon_entries_is_almost_zero = vector_with_epsilon_entries.is_almost_zero();
    /// let vector_with_entries_exceeding_epsilon_is_not_almost_zero = !vector_with_entries_exceeding_epsilon.is_almost_zero();
    ///
    /// assert!(zero_vector_is_almost_zero);
    /// assert!(vector_with_epsilon_entries_is_almost_zero);
    /// assert!(vector_with_entries_exceeding_epsilon_is_not_almost_zero);
    /// ```
    pub fn is_almost_zero(&self) -> bool {
        self.data.iter().all(|c| c.abs() <= EPSILON)
    }

    /// Calculates the cross product of this vector with another vector (`self x other`).
    ///
    /// Returns a new vector that is perpendicular to both input vectors,
    /// following the right-hand rule.
    ///
    /// # Example
    /// ```
    /// use bb_geometry::linear_algebra::vector3::*;
    /// let x_axis = Vector3::new([1.0, 0.0, 0.0]);
    /// let y_axis = Vector3::new([0.0, 1.0, 0.0]);
    /// let z_axis = Vector3::new([0.0, 0.0, 1.0]);
    ///
    /// let xy_cross = x_axis.cross(&y_axis);
    /// // x x y = z
    /// assert_eq!(xy_cross, z_axis);
    ///
    /// let yx_cross = y_axis.cross(&x_axis);
    /// // y x x = -z
    /// assert_eq!(yx_cross, -z_axis);
    ///
    /// let v1 = Vector3::new([1.0, 2.0, 3.0]);
    /// let v2 = Vector3::new([4.0, 5.0, 6.0]);
    /// let cross_v1_v2 = v1.cross(&v2);
    /// // x = 2*6 - 3*5 = 12 - 15 = -3
    /// // y = 3*4 - 1*6 = 12 - 6 = 6
    /// // z = 1*5 - 2*4 = 5 - 8 = -3
    /// assert_eq!(cross_v1_v2, Vector3::new([-3.0, 6.0, -3.0]));
    /// let t1 = cross_v1_v2.dot(&v1);
    /// let t2 = cross_v1_v2.dot(&v2);
    /// assert_eq!(t1, 0.0);
    /// assert_eq!(t2, 0.0);
    /// ```
    pub fn cross(&self, other: &Self) -> Self {
        Vector3::new([
            self.data[1] * other.data[2] - self.data[2] * other.data[1],
            self.data[2] * other.data[0] - self.data[0] * other.data[2],
            self.data[0] * other.data[1] - self.data[1] * other.data[0],
        ])
    }

    /// Calculates the squared magnitude (length squared) of the vector.
    /// Often useful as it avoids a square root calculation.
    /// result = x*x + y*y + z*z
    pub fn magnitude_squared(&self) -> f32 {
        self.dot(self)
    }

    /// Calculates the magnitude (length) of the vector.
    /// result = sqrt(x*x + y*y + z*z)
    pub fn magnitude(&self) -> f32 {
        self.magnitude_squared().sqrt()
    }

    /// Returns a new vector with the same direction but a magnitude of 1.
    /// Returns a zero vector if the original vector's magnitude is zero.
    pub fn normalize(&self) -> Self {
        let mag = self.magnitude();
        if mag == 0.0 {
            Self::zero()
        } else {
            self * (1.0 / mag)
        }
    }
}

// --- Trait Implementations ---

/// Allows accessing vector components using index `vector[index]`.
///
/// # Panics
/// Panics if `index` is out of bounds (>= 3).
impl Index<usize> for Vector3 {
    type Output = f32;

    #[inline]
    fn index(&self, index: usize) -> &Self::Output {
        assert!(index < 3, "Index out of bounds: {}", index);
        &self.data[index]
    }
}

/// Allows mutating vector components using index `vector[index] = value`.
///
/// # Panics
/// Panics if `index` is out of bounds (>= 3).
impl IndexMut<usize> for Vector3 {
    #[inline]
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        assert!(index < 3, "Index out of bounds: {}", index);
        &mut self.data[index]
    }
}

/// Implements vector addition using the `+` operator (`&Vector3 + &Vector3`).
impl Add<&Vector3> for &Vector3 {
    type Output = Vector3;

    fn add(self, rhs: &Vector3) -> Self::Output {
        let mut result_data = [0.0f32; 3];
        for i in 0..3 {
            result_data[i] = self.data[i] + rhs.data[i];
        }
        Vector3 { data: result_data }
    }
}
// Delegate owned + owned, owned + ref, ref + owned to ref + ref
impl Add<Vector3> for Vector3 {
    type Output = Vector3;
    #[inline]
    fn add(self, rhs: Vector3) -> Self::Output {
        &self + &rhs
    }
}
impl Add<&Vector3> for Vector3 {
    type Output = Vector3;
    #[inline]
    fn add(self, rhs: &Vector3) -> Self::Output {
        &self + rhs
    }
}
impl Add<Vector3> for &Vector3 {
    type Output = Vector3;
    #[inline]
    fn add(self, rhs: Vector3) -> Self::Output {
        self + &rhs
    }
}

/// Implements vector subtraction using the `-` operator (`&Vector3 - &Vector3`).
impl Sub<&Vector3> for &Vector3 {
    type Output = Vector3;

    fn sub(self, rhs: &Vector3) -> Self::Output {
        let mut result_data = [0.0f32; 3];
        for i in 0..3 {
            result_data[i] = self.data[i] - rhs.data[i];
        }
        Vector3 { data: result_data }
    }
}
// Delegate owned - owned, owned - ref, ref - owned to ref - ref
impl Sub<Vector3> for Vector3 {
    type Output = Vector3;
    #[inline]
    fn sub(self, rhs: Vector3) -> Self::Output {
        &self - &rhs
    }
}
impl Sub<&Vector3> for Vector3 {
    type Output = Vector3;
    #[inline]
    fn sub(self, rhs: &Vector3) -> Self::Output {
        &self - rhs
    }
}
impl Sub<Vector3> for &Vector3 {
    type Output = Vector3;
    #[inline]
    fn sub(self, rhs: Vector3) -> Self::Output {
        self - &rhs
    }
}

/// Implements vector negation using the unary `-` operator (`-vector`).
impl Neg for Vector3 {
    type Output = Self;

    fn neg(self) -> Self::Output {
        -&self
    }
}

impl Neg for &Vector3 {
    type Output = Vector3;

    fn neg(self) -> Self::Output {
        let mut result_data = [0.0f32; 3];
        for i in 0..3 {
            result_data[i] = -self.data[i];
        }
        Vector3 { data: result_data }
    }
}

/// Implements scalar multiplication (`&Vector3 * f32`).
impl Mul<f32> for &Vector3 {
    type Output = Vector3;

    fn mul(self, rhs: f32) -> Self::Output {
        let mut result_data = [0.0f32; 3];
        for i in 0..3 {
            result_data[i] = self.data[i] * rhs;
        }
        Vector3 { data: result_data }
    }
}
// Delegate owned * scalar to ref * scalar
impl Mul<f32> for Vector3 {
    type Output = Vector3;
    #[inline]
    fn mul(self, rhs: f32) -> Self::Output {
        &self * rhs
    }
}

/// Implements scalar multiplication (`f32 * &Vector3`).
impl Mul<&Vector3> for f32 {
    type Output = Vector3;

    fn mul(self, rhs: &Vector3) -> Self::Output {
        // Reuse the Vector * scalar implementation
        rhs * self
    }
}
// Delegate scalar * owned to scalar * ref
impl Mul<Vector3> for f32 {
    type Output = Vector3;
    #[inline]
    fn mul(self, rhs: Vector3) -> Self::Output {
        self * &rhs
    }
}

/// Implements Matrix * Vector multiplication (`&Matrix3x3 * &Vector3`).
/// Treats the vector as a column vector.
impl Mul<&Vector3> for &Matrix3x3 {
    type Output = Vector3;

    fn mul(self, rhs: &Vector3) -> Self::Output {
        let mut result_data = [0.0f32; 3];
        for i in 0..3 {
            // Row of linear_algebra / element of result vector
            let mut sum = 0.0;
            for k in 0..3 {
                // Column of linear_algebra / element of input vector
                sum += self[(i, k)] * rhs.data[k];
            }
            result_data[i] = sum;
        }
        Vector3 { data: result_data }
    }
}

// Delegate other Matrix * Vector combinations if needed
impl Mul<Vector3> for Matrix3x3 {
    type Output = Vector3;
    #[inline]
    fn mul(self, rhs: Vector3) -> Self::Output {
        &self * &rhs
    }
}
impl Mul<&Vector3> for Matrix3x3 {
    type Output = Vector3;
    #[inline]
    fn mul(self, rhs: &Vector3) -> Self::Output {
        &self * rhs
    }
}
impl Mul<Vector3> for &Matrix3x3 {
    type Output = Vector3;
    #[inline]
    fn mul(self, rhs: Vector3) -> Self::Output {
        self * &rhs
    }
}

pub const E_X: Vector3 = Vector3 {
    data: [1.0, 0.0, 0.0],
};
pub const E_Y: Vector3 = Vector3 {
    data: [0.0, 1.0, 0.0],
};
pub const E_Z: Vector3 = Vector3 {
    data: [0.0, 0.0, 1.0],
};