#![no_std]
#![forbid(unsafe_code)]
#![warn(missing_docs)]
use embedded_f32_sqrt::sqrt;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[non_exhaustive]
pub enum VecError {
ZeroNorm,
NonFiniteValue,
DivisionByZero,
}
fn robust_norm<const N: usize>(c: [f32; N]) -> Result<f32, VecError> {
let mut max = 0.0_f32;
for v in c {
if !v.is_finite() {
return Err(VecError::NonFiniteValue);
}
let a = v.abs();
if a > max {
max = a;
}
}
if max == 0.0 {
return Ok(0.0);
}
let mut sq = 0.0_f32;
for v in c {
sq += v * v;
}
if sq.is_finite() && sq >= f32::MIN_POSITIVE {
return sqrt(sq).map_err(|_| VecError::NonFiniteValue);
}
let mut s = 0.0_f32;
for v in c {
let r = v / max;
s += r * r;
}
let n = sqrt(s).map_err(|_| VecError::NonFiniteValue)? * max;
if n.is_finite() {
Ok(n)
} else {
Err(VecError::NonFiniteValue)
}
}
macro_rules! impl_vec {
($T:ident, $N:literal, $($f:ident : $i:literal),+) => {
impl $T {
#[inline]
pub const fn new($($f: f32),+) -> Self {
Self { $($f),+ }
}
pub const ZERO: Self = Self { $($f: 0.0),+ };
pub const ONE: Self = Self { $($f: 1.0),+ };
#[inline]
pub const fn as_array(&self) -> [f32; $N] {
[$(self.$f),+]
}
#[inline]
pub const fn from_array(v: [f32; $N]) -> Self {
Self { $($f: v[$i]),+ }
}
#[inline]
pub fn dot(&self, rhs: &Self) -> f32 {
0.0 $(+ self.$f * rhs.$f)+
}
#[inline]
pub fn norm_sq(&self) -> f32 {
self.dot(self)
}
pub fn norm(&self) -> Result<f32, VecError> {
robust_norm(self.as_array())
}
pub fn normalize(&self) -> Result<Self, VecError> {
let n = self.norm()?;
if n == 0.0 {
return Err(VecError::ZeroNorm);
}
Ok(Self { $($f: self.$f / n),+ })
}
#[inline]
pub fn normalize_or_zero(&self) -> Self {
self.normalize().unwrap_or(Self::ZERO)
}
#[inline]
pub fn distance_sq(&self, rhs: &Self) -> f32 {
(*self - *rhs).norm_sq()
}
pub fn distance(&self, rhs: &Self) -> Result<f32, VecError> {
(*self - *rhs).norm()
}
#[inline]
pub fn lerp(&self, rhs: &Self, t: f32) -> Self {
Self { $($f: self.$f + t * (rhs.$f - self.$f)),+ }
}
#[inline]
pub fn hadamard(&self, rhs: &Self) -> Self {
Self { $($f: self.$f * rhs.$f),+ }
}
#[inline]
pub fn min(&self, rhs: &Self) -> Self {
Self { $($f: self.$f.min(rhs.$f)),+ }
}
#[inline]
pub fn max(&self, rhs: &Self) -> Self {
Self { $($f: self.$f.max(rhs.$f)),+ }
}
#[inline]
pub fn abs(&self) -> Self {
Self { $($f: self.$f.abs()),+ }
}
pub fn checked_div(&self, s: f32) -> Result<Self, VecError> {
if s == 0.0 {
return Err(VecError::DivisionByZero);
}
let r = *self / s;
if r.is_finite() { Ok(r) } else { Err(VecError::NonFiniteValue) }
}
pub fn project_onto(&self, rhs: &Self) -> Result<Self, VecError> {
let mut rhs_scale = 0.0_f32;
for v in rhs.as_array() {
if !v.is_finite() {
return Err(VecError::NonFiniteValue);
}
let a = v.abs();
if a > rhs_scale {
rhs_scale = a;
}
}
if rhs_scale == 0.0 {
return Err(VecError::ZeroNorm);
}
let mut self_scale = 0.0_f32;
for v in self.as_array() {
if !v.is_finite() {
return Err(VecError::NonFiniteValue);
}
let a = v.abs();
if a > self_scale {
self_scale = a;
}
}
if self_scale == 0.0 {
return Ok(Self::ZERO);
}
let direction = *rhs / rhs_scale;
let scaled_self = *self / self_scale;
let factor = scaled_self.dot(&direction) / direction.norm_sq();
let out = (direction * factor) * self_scale;
if out.is_finite() { Ok(out) } else { Err(VecError::NonFiniteValue) }
}
pub fn reject_from(&self, rhs: &Self) -> Result<Self, VecError> {
let out = *self - self.project_onto(rhs)?;
if out.is_finite() { Ok(out) } else { Err(VecError::NonFiniteValue) }
}
pub fn reflect(&self, normal: &Self) -> Result<Self, VecError> {
let out = *self - self.project_onto(normal)? * 2.0;
if out.is_finite() { Ok(out) } else { Err(VecError::NonFiniteValue) }
}
#[inline]
pub fn is_finite(&self) -> bool {
true $(&& self.$f.is_finite())+
}
#[inline]
pub fn is_approx_eq(&self, rhs: &Self, eps: f32) -> bool {
true $(&& (self.$f - rhs.$f).abs() <= eps)+
}
}
impl Default for $T {
#[inline]
fn default() -> Self { Self::ZERO }
}
impl From<[f32; $N]> for $T {
#[inline]
fn from(v: [f32; $N]) -> Self { Self::from_array(v) }
}
impl From<$T> for [f32; $N] {
#[inline]
fn from(v: $T) -> Self { v.as_array() }
}
impl core::ops::Add for $T {
type Output = Self;
#[inline]
fn add(self, rhs: Self) -> Self { Self { $($f: self.$f + rhs.$f),+ } }
}
impl core::ops::Sub for $T {
type Output = Self;
#[inline]
fn sub(self, rhs: Self) -> Self { Self { $($f: self.$f - rhs.$f),+ } }
}
impl core::ops::Mul<f32> for $T {
type Output = Self;
#[inline]
fn mul(self, s: f32) -> Self { Self { $($f: self.$f * s),+ } }
}
impl core::ops::Mul<$T> for f32 {
type Output = $T;
#[inline]
fn mul(self, v: $T) -> $T { v * self }
}
impl core::ops::Div<f32> for $T {
type Output = Self;
#[inline]
fn div(self, s: f32) -> Self { Self { $($f: self.$f / s),+ } }
}
impl core::ops::Neg for $T {
type Output = Self;
#[inline]
fn neg(self) -> Self { Self { $($f: -self.$f),+ } }
}
impl core::ops::AddAssign for $T {
#[inline]
fn add_assign(&mut self, rhs: Self) { $(self.$f += rhs.$f;)+ }
}
impl core::ops::SubAssign for $T {
#[inline]
fn sub_assign(&mut self, rhs: Self) { $(self.$f -= rhs.$f;)+ }
}
impl core::ops::MulAssign<f32> for $T {
#[inline]
fn mul_assign(&mut self, s: f32) { $(self.$f *= s;)+ }
}
impl core::ops::DivAssign<f32> for $T {
#[inline]
fn div_assign(&mut self, s: f32) { $(self.$f /= s;)+ }
}
impl core::iter::Sum for $T {
fn sum<I: Iterator<Item = Self>>(iter: I) -> Self {
iter.fold(Self::ZERO, |a, b| a + b)
}
}
};
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Vec2 {
pub x: f32,
pub y: f32,
}
impl_vec!(Vec2, 2, x: 0, y: 1);
impl Vec2 {
pub const X: Self = Self::new(1.0, 0.0);
pub const Y: Self = Self::new(0.0, 1.0);
#[inline]
pub fn cross(&self, rhs: &Self) -> f32 {
self.x * rhs.y - self.y * rhs.x
}
#[inline]
pub const fn perp(&self) -> Self {
Self::new(-self.y, self.x)
}
#[inline]
pub const fn extend(&self, z: f32) -> Vec3 {
Vec3::new(self.x, self.y, z)
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Vec3 {
pub x: f32,
pub y: f32,
pub z: f32,
}
impl_vec!(Vec3, 3, x: 0, y: 1, z: 2);
impl Vec3 {
pub const X: Self = Self::new(1.0, 0.0, 0.0);
pub const Y: Self = Self::new(0.0, 1.0, 0.0);
pub const Z: Self = Self::new(0.0, 0.0, 1.0);
#[inline]
pub fn cross(&self, rhs: &Self) -> Self {
Self::new(
self.y * rhs.z - self.z * rhs.y,
self.z * rhs.x - self.x * rhs.z,
self.x * rhs.y - self.y * rhs.x,
)
}
#[inline]
pub fn triple(&self, b: &Self, c: &Self) -> f32 {
self.dot(&b.cross(c))
}
#[inline]
pub const fn truncate(&self) -> Vec2 {
Vec2::new(self.x, self.y)
}
#[inline]
pub const fn extend(&self, w: f32) -> Vec4 {
Vec4::new(self.x, self.y, self.z, w)
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Vec4 {
pub x: f32,
pub y: f32,
pub z: f32,
pub w: f32,
}
impl_vec!(Vec4, 4, x: 0, y: 1, z: 2, w: 3);
impl Vec4 {
pub const X: Self = Self::new(1.0, 0.0, 0.0, 0.0);
pub const Y: Self = Self::new(0.0, 1.0, 0.0, 0.0);
pub const Z: Self = Self::new(0.0, 0.0, 1.0, 0.0);
pub const W: Self = Self::new(0.0, 0.0, 0.0, 1.0);
#[inline]
pub const fn from_vec3(v: Vec3, w: f32) -> Self {
Self::new(v.x, v.y, v.z, w)
}
#[inline]
pub const fn xyz(&self) -> Vec3 {
Vec3::new(self.x, self.y, self.z)
}
pub fn perspective_divide(&self) -> Result<Vec3, VecError> {
self.xyz().checked_div(self.w)
}
}