1037 lines
27 KiB
Rust
1037 lines
27 KiB
Rust
use super::traits::{SqrtExt, Tuple, VectorLike};
|
|
use super::{Float, NumFloat, PI};
|
|
use crate::utils::interval::Interval;
|
|
use crate::utils::math::{clamp, difference_of_products, quadratic, safe_asin};
|
|
use core::fmt;
|
|
use core::hash::{Hash, Hasher};
|
|
use core::iter::Sum;
|
|
use core::ops::{
|
|
Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign,
|
|
};
|
|
use num_traits::{AsPrimitive, FloatConst, Num, Signed, Zero};
|
|
|
|
pub trait MulAdd<M = Self, A = Self> {
|
|
type Output;
|
|
fn mul_add(self, multiplier: M, addend: A) -> Self::Output;
|
|
}
|
|
|
|
impl MulAdd<Float, Float> for Float {
|
|
type Output = Float;
|
|
#[inline(always)]
|
|
fn mul_add(self, multiplier: Float, addend: Float) -> Self::Output {
|
|
num_traits::Float::mul_add(self, multiplier, addend)
|
|
}
|
|
}
|
|
|
|
impl MulAdd<f64, f64> for f64 {
|
|
type Output = f64;
|
|
#[inline(always)]
|
|
fn mul_add(self, multiplier: f64, addend: f64) -> Self::Output {
|
|
num_traits::Float::mul_add(self, multiplier, addend)
|
|
}
|
|
}
|
|
|
|
// N-dimensional displacement
|
|
#[repr(C)]
|
|
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
|
|
pub struct Vector<T, const N: usize>(pub [T; N]);
|
|
// N-dimensional location
|
|
#[repr(C)]
|
|
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
|
|
pub struct Point<T, const N: usize>(pub [T; N]);
|
|
// N-dimensional surface normal
|
|
#[repr(C)]
|
|
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
|
|
pub struct Normal<T, const N: usize>(pub [T; N]);
|
|
|
|
impl<T: fmt::Display, const N: usize> fmt::Display for Vector<T, N> {
|
|
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
|
|
write!(f, "Vector(")?;
|
|
for (i, item) in (&self.0).into_iter().enumerate() {
|
|
if i > 0 {
|
|
write!(f, ", ")?;
|
|
}
|
|
write!(f, "{}", item)?;
|
|
}
|
|
write!(f, ")")
|
|
}
|
|
}
|
|
|
|
impl<T: fmt::Display, const N: usize> fmt::Display for Point<T, N> {
|
|
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
|
|
write!(f, "Point(")?;
|
|
for (i, item) in (&self.0).into_iter().enumerate() {
|
|
if i > 0 {
|
|
write!(f, ", ")?;
|
|
}
|
|
write!(f, "{}", item)?;
|
|
}
|
|
write!(f, ")")
|
|
}
|
|
}
|
|
|
|
impl<T: fmt::Display, const N: usize> fmt::Display for Normal<T, N> {
|
|
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
|
|
write!(f, "Normal(")?;
|
|
for (i, item) in (&self.0).into_iter().enumerate() {
|
|
if i > 0 {
|
|
write!(f, ", ")?;
|
|
}
|
|
write!(f, "{}", item)?;
|
|
}
|
|
write!(f, ")")
|
|
}
|
|
}
|
|
|
|
#[macro_export]
|
|
macro_rules! impl_tuple_core {
|
|
($Struct:ident) => {
|
|
impl<T: Copy, const N: usize> Tuple<T, N> for $Struct<T, N> {
|
|
#[inline]
|
|
fn data(&self) -> &[T; N] {
|
|
&self.0
|
|
}
|
|
|
|
#[inline]
|
|
fn data_mut(&mut self) -> &mut [T; N] {
|
|
&mut self.0
|
|
}
|
|
|
|
#[inline]
|
|
fn from_array(arr: [T; N]) -> Self {
|
|
Self(arr)
|
|
}
|
|
}
|
|
|
|
impl<T: Default + Copy, const N: usize> Default for $Struct<T, N> {
|
|
fn default() -> Self {
|
|
Self([T::default(); N])
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> $Struct<T, N>
|
|
where
|
|
T: Zero + Copy,
|
|
{
|
|
#[inline]
|
|
pub fn zero() -> Self {
|
|
Self([T::zero(); N])
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> From<T> for $Struct<T, N>
|
|
where
|
|
T: Copy,
|
|
{
|
|
#[inline]
|
|
fn from(scalar: T) -> Self {
|
|
Self([scalar; N])
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> $Struct<f32, N> {
|
|
#[inline]
|
|
pub fn floor(&self) -> $Struct<i32, N> {
|
|
$Struct(self.0.map(|v| v.floor() as i32))
|
|
}
|
|
|
|
#[inline]
|
|
pub fn average(&self) -> f32 {
|
|
let sum: f32 = self.0.iter().sum();
|
|
sum / (N as f32)
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> $Struct<T, N>
|
|
where
|
|
T: Copy + PartialOrd,
|
|
{
|
|
#[inline]
|
|
pub fn min(&self, other: Self) -> Self {
|
|
let mut out = self.0;
|
|
for i in 0..N {
|
|
if other.0[i] < out[i] {
|
|
out[i] = other.0[i];
|
|
}
|
|
}
|
|
Self(out)
|
|
}
|
|
|
|
#[inline]
|
|
pub fn max(&self, other: Self) -> Self {
|
|
let mut out = self.0;
|
|
for i in 0..N {
|
|
if other.0[i] > out[i] {
|
|
out[i] = other.0[i]
|
|
}
|
|
}
|
|
Self(out)
|
|
}
|
|
|
|
#[inline]
|
|
pub fn max_component_value(&self) -> T {
|
|
let mut m = self.0[0];
|
|
for i in 1..N {
|
|
if self.0[i] > m {
|
|
m = self.0[i];
|
|
}
|
|
}
|
|
m
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> $Struct<T, N>
|
|
where
|
|
T: Copy,
|
|
{
|
|
#[inline]
|
|
pub fn fill(value: T) -> Self {
|
|
Self([value; N])
|
|
}
|
|
|
|
#[inline]
|
|
pub fn cast<U>(&self) -> $Struct<U, N>
|
|
where
|
|
U: 'static + Copy,
|
|
T: 'static + Copy + AsPrimitive<U>,
|
|
{
|
|
$Struct(self.0.map(|c| c.as_()))
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> Index<usize> for $Struct<T, N> {
|
|
type Output = T;
|
|
#[inline]
|
|
fn index(&self, index: usize) -> &Self::Output {
|
|
&self.0[index]
|
|
}
|
|
}
|
|
impl<T, const N: usize> IndexMut<usize> for $Struct<T, N> {
|
|
#[inline]
|
|
fn index_mut(&mut self, index: usize) -> &mut Self::Output {
|
|
&mut self.0[index]
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> Neg for $Struct<T, N>
|
|
where
|
|
T: Neg<Output = T> + Copy,
|
|
{
|
|
type Output = Self;
|
|
fn neg(self) -> Self::Output {
|
|
Self(self.0.map(|c| -c))
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
#[macro_export]
|
|
macro_rules! impl_num_zero {
|
|
($Struct:ident) => {
|
|
impl<T, const N: usize> num_traits::Zero for $Struct<T, N>
|
|
where
|
|
T: num_traits::Zero + Copy + PartialEq,
|
|
{
|
|
#[inline]
|
|
fn zero() -> Self {
|
|
Self([T::zero(); N])
|
|
}
|
|
#[inline]
|
|
fn is_zero(&self) -> bool {
|
|
self.0.iter().all(|c| c.is_zero())
|
|
}
|
|
}
|
|
};
|
|
}
|
|
impl_num_zero!(Vector);
|
|
impl_num_zero!(Normal);
|
|
|
|
#[macro_export]
|
|
macro_rules! impl_scalar_ops {
|
|
($Struct:ident) => {
|
|
impl<T, const N: usize> Mul<T> for $Struct<T, N>
|
|
where
|
|
T: Mul<Output = T> + Copy,
|
|
{
|
|
type Output = Self;
|
|
fn mul(self, rhs: T) -> Self::Output {
|
|
let mut result = self.0;
|
|
for i in 0..N {
|
|
result[i] = result[i] * rhs;
|
|
}
|
|
Self(result)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Mul<$Struct<Float, N>> for Float {
|
|
type Output = $Struct<Float, N>;
|
|
fn mul(self, rhs: $Struct<Float, N>) -> Self::Output {
|
|
rhs * self
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> MulAssign<T> for $Struct<T, N>
|
|
where
|
|
T: MulAssign + Copy,
|
|
{
|
|
fn mul_assign(&mut self, rhs: T) {
|
|
for i in 0..N {
|
|
self.0[i] *= rhs;
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> Div<T> for $Struct<T, N>
|
|
where
|
|
T: Div<Output = T> + Copy,
|
|
{
|
|
type Output = Self;
|
|
fn div(self, rhs: T) -> Self::Output {
|
|
let mut result = self.0;
|
|
for i in 0..N {
|
|
result[i] = result[i] / rhs;
|
|
}
|
|
Self(result)
|
|
}
|
|
}
|
|
impl<T, const N: usize> DivAssign<T> for $Struct<T, N>
|
|
where
|
|
T: DivAssign + Copy,
|
|
{
|
|
fn div_assign(&mut self, rhs: T) {
|
|
for i in 0..N {
|
|
self.0[i] /= rhs;
|
|
}
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
#[macro_export]
|
|
macro_rules! impl_op {
|
|
($Op:ident, $op:ident, $Lhs:ident, $Rhs:ident, $Output:ident) => {
|
|
impl<T, const N: usize> $Op<$Rhs<T, N>> for $Lhs<T, N>
|
|
where
|
|
T: $Op<Output = T> + Copy,
|
|
{
|
|
type Output = $Output<T, N>;
|
|
fn $op(self, rhs: $Rhs<T, N>) -> Self::Output {
|
|
let mut result = self.0;
|
|
for i in 0..N {
|
|
result[i] = $Op::$op(self.0[i], rhs.0[i]);
|
|
}
|
|
$Output(result)
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
#[macro_export]
|
|
macro_rules! impl_op_assign {
|
|
($OpAssign:ident, $op_assign:ident, $Lhs:ident, $Rhs:ident) => {
|
|
impl<T, const N: usize> $OpAssign<$Rhs<T, N>> for $Lhs<T, N>
|
|
where
|
|
T: $OpAssign + Copy,
|
|
{
|
|
fn $op_assign(&mut self, rhs: $Rhs<T, N>) {
|
|
for i in 0..N {
|
|
$OpAssign::$op_assign(&mut self.0[i], rhs.0[i]);
|
|
}
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
#[macro_export]
|
|
macro_rules! impl_mul_add {
|
|
($Struct:ident) => {
|
|
impl<T, const N: usize> MulAdd<T, $Struct<T, N>> for $Struct<T, N>
|
|
where
|
|
T: MulAdd<T, T, Output = T> + Copy,
|
|
{
|
|
type Output = $Struct<T, N>;
|
|
|
|
#[inline(always)]
|
|
fn mul_add(self, multiplier: T, addend: $Struct<T, N>) -> Self::Output {
|
|
let mut result = self.0;
|
|
for i in 0..N {
|
|
result[i] = self.0[i].mul_add(multiplier, addend.0[i]);
|
|
}
|
|
Self(result)
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
#[macro_export]
|
|
macro_rules! impl_float_vector_ops {
|
|
($Struct:ident) => {
|
|
impl<T, const N: usize> VectorLike for $Struct<T, N>
|
|
where
|
|
T: Copy
|
|
+ Zero
|
|
+ Add<Output = T>
|
|
+ Mul<Output = T>
|
|
+ Sub<Output = T>
|
|
+ Div<Output = T>
|
|
+ SqrtExt,
|
|
{
|
|
type Scalar = T;
|
|
fn dot(self, rhs: Self) -> T {
|
|
let mut sum = T::zero();
|
|
for i in 0..N {
|
|
sum = sum + self[i] * rhs[i];
|
|
}
|
|
sum
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
macro_rules! impl_tuple_conversions {
|
|
($Struct:ident) => {
|
|
impl<T: NumFloat> From<(T, T, T)> for $Struct<T, 3> {
|
|
fn from(tuple: (T, T, T)) -> Self {
|
|
Self([tuple.0, tuple.1, tuple.2])
|
|
}
|
|
}
|
|
|
|
// Allow converting (x, y) -> Vector<T, 2>
|
|
impl<T: NumFloat> From<(T, T)> for $Struct<T, 2> {
|
|
fn from(tuple: (T, T)) -> Self {
|
|
Self([tuple.0, tuple.1])
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
macro_rules! impl_abs {
|
|
($Struct:ident) => {
|
|
impl<T, const N: usize> $Struct<T, N>
|
|
where
|
|
T: Signed + Copy,
|
|
{
|
|
pub fn abs(self) -> Self {
|
|
let mut result = self.0;
|
|
for i in 0..N {
|
|
result[i] = result[i].abs();
|
|
}
|
|
Self(result)
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
macro_rules! impl_accessors {
|
|
($Struct:ident) => {
|
|
impl<T: Copy> $Struct<T, 2> {
|
|
pub fn x(&self) -> T {
|
|
self.0[0]
|
|
}
|
|
pub fn y(&self) -> T {
|
|
self.0[1]
|
|
}
|
|
}
|
|
impl<T: Copy> $Struct<T, 3> {
|
|
pub fn x(&self) -> T {
|
|
self.0[0]
|
|
}
|
|
pub fn y(&self) -> T {
|
|
self.0[1]
|
|
}
|
|
pub fn z(&self) -> T {
|
|
self.0[2]
|
|
}
|
|
}
|
|
};
|
|
}
|
|
|
|
impl_tuple_core!(Vector);
|
|
impl_tuple_core!(Point);
|
|
impl_tuple_core!(Normal);
|
|
|
|
impl_scalar_ops!(Vector);
|
|
impl_scalar_ops!(Normal);
|
|
|
|
// Addition
|
|
impl_op!(Add, add, Vector, Vector, Vector);
|
|
impl_op!(Add, add, Point, Vector, Point);
|
|
impl_op!(Add, add, Vector, Point, Point);
|
|
impl_op!(Add, add, Normal, Normal, Normal);
|
|
|
|
// Subtraction
|
|
impl_op!(Sub, sub, Vector, Vector, Vector);
|
|
impl_op!(Sub, sub, Point, Vector, Point);
|
|
impl_op!(Sub, sub, Point, Point, Vector);
|
|
impl_op!(Sub, sub, Normal, Normal, Normal);
|
|
|
|
// AddAssign
|
|
impl_op_assign!(AddAssign, add_assign, Vector, Vector);
|
|
impl_op_assign!(AddAssign, add_assign, Point, Vector);
|
|
impl_op_assign!(AddAssign, add_assign, Normal, Normal);
|
|
|
|
// SubAssign
|
|
impl_op_assign!(SubAssign, sub_assign, Vector, Vector);
|
|
impl_op_assign!(SubAssign, sub_assign, Point, Vector);
|
|
impl_op_assign!(SubAssign, sub_assign, Normal, Normal);
|
|
|
|
impl_float_vector_ops!(Vector);
|
|
impl_float_vector_ops!(Normal);
|
|
impl_abs!(Vector);
|
|
impl_abs!(Normal);
|
|
impl_abs!(Point);
|
|
impl_accessors!(Vector);
|
|
impl_accessors!(Point);
|
|
impl_accessors!(Normal);
|
|
|
|
impl_mul_add!(Vector);
|
|
impl_mul_add!(Point);
|
|
impl_mul_add!(Normal);
|
|
|
|
// Convert from tuple of Floats, for parsing issues
|
|
impl_tuple_conversions!(Vector);
|
|
impl_tuple_conversions!(Point);
|
|
impl_tuple_conversions!(Normal);
|
|
|
|
impl<T: Copy, const N: usize> From<Vector<T, N>> for Normal<T, N> {
|
|
fn from(v: Vector<T, N>) -> Self {
|
|
Self(v.0)
|
|
}
|
|
}
|
|
impl<T: Copy, const N: usize> From<Normal<T, N>> for Vector<T, N> {
|
|
fn from(n: Normal<T, N>) -> Self {
|
|
Self(n.0)
|
|
}
|
|
}
|
|
|
|
impl<T: Copy, const N: usize> From<Vector<T, N>> for Point<T, N> {
|
|
fn from(v: Vector<T, N>) -> Self {
|
|
Self(v.0)
|
|
}
|
|
}
|
|
|
|
impl<T: Copy, const N: usize> From<Point<T, N>> for Vector<T, N> {
|
|
fn from(n: Point<T, N>) -> Self {
|
|
Self(n.0)
|
|
}
|
|
}
|
|
|
|
impl<T, const N: usize> Point<T, N>
|
|
where
|
|
T: NumFloat + SqrtExt,
|
|
{
|
|
pub fn distance(self, other: Self) -> T {
|
|
(self - other).norm()
|
|
}
|
|
|
|
pub fn distance_squared(self, other: Self) -> T {
|
|
(self - other).norm_squared()
|
|
}
|
|
}
|
|
|
|
impl Point2f {
|
|
pub fn invert_bilinear(p: Point2f, vert: &[Point2f]) -> Point2f {
|
|
let a = vert[0];
|
|
let b = vert[1];
|
|
let c = vert[3];
|
|
let d = vert[2];
|
|
let e = b - a;
|
|
let f = d - a;
|
|
let g = (a - b) + (c - d);
|
|
let h = p - a;
|
|
|
|
let cross2d = |a: Vector2f, b: Vector2f| difference_of_products(a.x(), b.y(), a.y(), b.x());
|
|
|
|
let k2 = cross2d(g, f);
|
|
let k1 = cross2d(e, f) + cross2d(h, g);
|
|
let k0 = cross2d(h, e);
|
|
|
|
// if edges are parallel, this is a linear equation
|
|
if k2.abs() < 0.001 {
|
|
if (e.x() * k1 - g.x() * k0).abs() < 1e-5 {
|
|
return Point2f::new(
|
|
(h.y() * k1 + f.y() * k0) / (e.y() * k1 - g.y() * k0),
|
|
-k0 / k1,
|
|
);
|
|
} else {
|
|
return Point2f::new(
|
|
(h.x() * k1 + f.x() * k0) / (e.x() * k1 - g.x() * k0),
|
|
-k0 / k1,
|
|
);
|
|
}
|
|
}
|
|
|
|
if let Some((v0, v1)) = quadratic(k2, k1, k0) {
|
|
let u = (h.x() - f.x() * v0) / (e.x() + g.x() * v0);
|
|
if !(0.0..=1.).contains(&u) || !(0.0..=1.0).contains(&v0) {
|
|
return Point2f::new((h.x() - f.x() * v1) / (e.x() + g.x() * v1), v1);
|
|
}
|
|
Point2f::new(u, v0)
|
|
} else {
|
|
Point2f::zero()
|
|
}
|
|
}
|
|
}
|
|
|
|
// Utility aliases and functions
|
|
pub type Point2<T> = Point<T, 2>;
|
|
pub type Point2f = Point2<Float>;
|
|
pub type Point2i = Point2<i32>;
|
|
pub type Point2fi = Point2<Interval>;
|
|
pub type Point3<T> = Point<T, 3>;
|
|
pub type Point3f = Point3<Float>;
|
|
pub type Point3i = Point3<i32>;
|
|
pub type Point3fi = Point3<Interval>;
|
|
pub type Vector2<T> = Vector<T, 2>;
|
|
pub type Vector2f = Vector2<Float>;
|
|
pub type Vector2i = Vector2<i32>;
|
|
pub type Vector2fi = Vector2<Interval>;
|
|
pub type Vector3<T> = Vector<T, 3>;
|
|
pub type Vector3f = Vector3<Float>;
|
|
pub type Vector3i = Vector3<i32>;
|
|
pub type Vector3fi = Vector3<Interval>;
|
|
pub type Normal2<T> = Normal<T, 2>;
|
|
pub type Normal3<T> = Normal<T, 3>;
|
|
pub type Normal3f = Normal3<Float>;
|
|
pub type Normal3i = Normal3<i32>;
|
|
pub type Vector4<T> = Vector<T, 4>;
|
|
|
|
impl<T: Copy> Vector2<T> {
|
|
pub fn new(x: T, y: T) -> Self {
|
|
Self([x, y])
|
|
}
|
|
}
|
|
impl<T: Copy> Point2<T> {
|
|
pub fn new(x: T, y: T) -> Self {
|
|
Self([x, y])
|
|
}
|
|
}
|
|
impl<T: Copy> Normal2<T> {
|
|
pub fn new(x: T, y: T) -> Self {
|
|
Self([x, y])
|
|
}
|
|
}
|
|
|
|
impl<T: Copy> Vector3<T> {
|
|
pub fn new(x: T, y: T, z: T) -> Self {
|
|
Self([x, y, z])
|
|
}
|
|
}
|
|
impl<T: Copy> Point3<T> {
|
|
pub fn new(x: T, y: T, z: T) -> Self {
|
|
Self([x, y, z])
|
|
}
|
|
}
|
|
impl<T: Copy> Normal3<T> {
|
|
pub fn new(x: T, y: T, z: T) -> Self {
|
|
Self([x, y, z])
|
|
}
|
|
}
|
|
impl<T: Copy> Vector4<T> {
|
|
pub fn new(x: T, y: T, z: T, w: T) -> Self {
|
|
Self([x, y, z, w])
|
|
}
|
|
}
|
|
|
|
// Vector operations
|
|
impl<T> Vector3<T>
|
|
where
|
|
T: Num + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
|
|
{
|
|
pub fn cross(self, rhs: Self) -> Self {
|
|
Self([
|
|
difference_of_products(self[1], rhs[2], self[2], rhs[1]),
|
|
difference_of_products(self[2], rhs[0], self[0], rhs[2]),
|
|
difference_of_products(self[0], rhs[1], self[1], rhs[0]),
|
|
])
|
|
}
|
|
}
|
|
|
|
impl<T> Normal3<T>
|
|
where
|
|
T: Num + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
|
|
{
|
|
pub fn cross(self, rhs: Self) -> Self {
|
|
Self([
|
|
difference_of_products(self[1], rhs[2], self[2], rhs[1]),
|
|
difference_of_products(self[2], rhs[0], self[0], rhs[2]),
|
|
difference_of_products(self[0], rhs[1], self[1], rhs[0]),
|
|
])
|
|
}
|
|
}
|
|
|
|
impl<T> Vector3<T>
|
|
where
|
|
T: Num + NumFloat + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
|
|
{
|
|
pub fn coordinate_system(&self) -> (Self, Self)
|
|
where
|
|
T: NumFloat,
|
|
{
|
|
let v2 = if self[0].abs() > self[1].abs() {
|
|
Self::new(-self[2], T::zero(), self[0]) / (self[0] * self[0] + self[2] * self[2]).sqrt()
|
|
} else {
|
|
Self::new(T::zero(), self[2], -self[1]) / (self[1] * self[1] + self[2] * self[2]).sqrt()
|
|
};
|
|
(v2, self.cross(v2))
|
|
}
|
|
|
|
pub fn coordinate_system_from_cpp(&self) -> (Self, Self) {
|
|
let sign = self.z().copysign(T::one());
|
|
let a = -T::one() / (sign + self.z());
|
|
let b = self.x() * self.y() * a;
|
|
let v2 = Self::new(
|
|
T::one() + sign * self.x().powi(2) * a,
|
|
sign * b,
|
|
-sign * self.x(),
|
|
);
|
|
let v3 = Self::new(b, sign + self.y().powi(2) * a, -self.y());
|
|
(v2, v3)
|
|
}
|
|
}
|
|
|
|
impl<T> Normal3<T>
|
|
where
|
|
T: Num + NumFloat + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
|
|
{
|
|
pub fn coordinate_system(&self) -> (Self, Self)
|
|
where
|
|
T: NumFloat,
|
|
{
|
|
let v2 = if self[0].abs() > self[1].abs() {
|
|
Self::new(-self[2], T::zero(), self[0]) / (self[0] * self[0] + self[2] * self[2]).sqrt()
|
|
} else {
|
|
Self::new(T::zero(), self[2], -self[1]) / (self[1] * self[1] + self[2] * self[2]).sqrt()
|
|
};
|
|
(v2, self.cross(v2))
|
|
}
|
|
|
|
pub fn coordinate_system_from_cpp(&self) -> (Self, Self) {
|
|
let sign = self.z().copysign(T::one());
|
|
let a = -T::one() / (sign + self.z());
|
|
let b = self.x() * self.y() * a;
|
|
let v2 = Self::new(
|
|
T::one() + sign * self.x().powi(2) * a,
|
|
sign * b,
|
|
-sign * self.x(),
|
|
);
|
|
let v3 = Self::new(b, sign + self.y().powi(2) * a, -self.y());
|
|
(v2, v3)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Hash for Vector<Float, N> {
|
|
fn hash<H: Hasher>(&self, state: &mut H) {
|
|
for item in self.0.iter() {
|
|
item.to_bits().hash(state);
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Hash for Point<Float, N> {
|
|
fn hash<H: Hasher>(&self, state: &mut H) {
|
|
for item in self.0.iter() {
|
|
item.to_bits().hash(state);
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Hash for Normal<Float, N> {
|
|
fn hash<H: Hasher>(&self, state: &mut H) {
|
|
for item in self.0.iter() {
|
|
item.to_bits().hash(state);
|
|
}
|
|
}
|
|
}
|
|
|
|
// INTERVAL STUFF
|
|
impl<const N: usize> Point<Interval, N> {
|
|
pub fn new_from_point(p: Point<Float, N>) -> Self {
|
|
let mut arr = [Interval::default(); N];
|
|
for i in 0..N {
|
|
arr[i] = Interval::new(p[i]);
|
|
}
|
|
Self(arr)
|
|
}
|
|
|
|
pub fn new_with_error(p: Point<Float, N>, e: Vector<Float, N>) -> Self {
|
|
let mut arr = [Interval::default(); N];
|
|
for i in 0..N {
|
|
arr[i] = Interval::new_from_value_and_error(p[i], e[i]);
|
|
}
|
|
Self(arr)
|
|
}
|
|
|
|
pub fn error(&self) -> Vector<Float, N> {
|
|
let mut arr = [0.0; N];
|
|
for i in 0..N {
|
|
arr[i] = self[i].width() / 2.0;
|
|
}
|
|
Vector(arr)
|
|
}
|
|
|
|
pub fn midpoint(&self) -> Point<Float, N> {
|
|
let mut arr = [0.0; N];
|
|
for i in 0..N {
|
|
arr[i] = self[i].midpoint();
|
|
}
|
|
Point(arr)
|
|
}
|
|
|
|
pub fn is_exact(&self) -> bool {
|
|
self.0.iter().all(|interval| interval.width() == 0.0)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Vector<Interval, N> {
|
|
pub fn new_from_vector(v: Vector<Float, N>) -> Self {
|
|
let mut arr = [Interval::default(); N];
|
|
for i in 0..N {
|
|
arr[i] = Interval::new(v[i]);
|
|
}
|
|
Self(arr)
|
|
}
|
|
|
|
pub fn new_with_error(v: Vector<Float, N>, e: Vector<Float, N>) -> Self {
|
|
let mut arr = [Interval::default(); N];
|
|
for i in 0..N {
|
|
arr[i] = Interval::new_from_value_and_error(v[i], e[i]);
|
|
}
|
|
Self(arr)
|
|
}
|
|
|
|
pub fn error(&self) -> Vector<Float, N> {
|
|
let mut arr = [0.0; N];
|
|
for i in 0..N {
|
|
arr[i] = self[i].width() / 2.0;
|
|
}
|
|
Vector(arr)
|
|
}
|
|
|
|
pub fn midpoint(&self) -> Vector<Float, N> {
|
|
let mut arr = [0.0; N];
|
|
for i in 0..N {
|
|
arr[i] = self[i].midpoint();
|
|
}
|
|
Vector(arr)
|
|
}
|
|
|
|
pub fn is_exact(&self) -> bool {
|
|
self.0.iter().all(|interval| interval.width() == 0.0)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> From<Point<Interval, N>> for Point<Float, N> {
|
|
fn from(pi: Point<Interval, N>) -> Self {
|
|
let mut arr = [0.0; N];
|
|
for i in 0..N {
|
|
arr[i] = pi[i].midpoint();
|
|
}
|
|
Point(arr)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> From<Vector<Interval, N>> for Vector<Float, N> {
|
|
fn from(pi: Vector<Interval, N>) -> Self {
|
|
let mut arr = [0.0; N];
|
|
for i in 0..N {
|
|
arr[i] = pi[i].midpoint();
|
|
}
|
|
Vector(arr)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> From<Vector<Float, N>> for Vector<Interval, N> {
|
|
fn from(v: Vector<Float, N>) -> Self {
|
|
let mut arr = [Interval::default(); N];
|
|
for i in 0..N {
|
|
arr[i] = Interval::new(v[i]);
|
|
}
|
|
Self(arr)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Mul<Vector<Interval, N>> for Interval {
|
|
type Output = Vector<Interval, N>;
|
|
fn mul(self, rhs: Vector<Interval, N>) -> Self::Output {
|
|
rhs * self
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Div<Vector<Interval, N>> for Interval {
|
|
type Output = Vector<Interval, N>;
|
|
fn div(self, rhs: Vector<Interval, N>) -> Self::Output {
|
|
let mut result = rhs.0;
|
|
for i in 0..N {
|
|
result[i] = self / rhs[i];
|
|
}
|
|
Vector(result)
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> From<Vector<i32, N>> for Vector<f32, N> {
|
|
fn from(v: Vector<i32, N>) -> Self {
|
|
Self(v.0.map(|c| c as f32))
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> From<Point<i32, N>> for Point<Float, N> {
|
|
fn from(p: Point<i32, N>) -> Self {
|
|
Point(p.0.map(|c| c as Float))
|
|
}
|
|
}
|
|
|
|
impl<T> Normal3<T>
|
|
where
|
|
T: Num + PartialOrd + Copy + Neg<Output = T> + SqrtExt,
|
|
{
|
|
pub fn face_forward(self, v: impl Into<Vector3<T>>) -> Self {
|
|
let v: Vector3<T> = v.into();
|
|
if Vector3::<T>::from(self).dot(v) < T::zero() {
|
|
-self
|
|
} else {
|
|
self
|
|
}
|
|
}
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
|
|
pub struct OctahedralVector {
|
|
x: u16,
|
|
y: u16,
|
|
}
|
|
|
|
impl OctahedralVector {
|
|
pub fn new(mut v: Vector3f) -> Self {
|
|
v /= v.x().abs() + v.y().abs() + v.z().abs();
|
|
|
|
let (x_enc, y_enc) = if v.z() >= 0.0 {
|
|
(Self::encode(v.x()), Self::encode(v.y()))
|
|
} else {
|
|
(
|
|
Self::encode((1.0 - v.y().abs()) * Self::sign(v.x())),
|
|
Self::encode((1.0 - v.x().abs()) * Self::sign(v.y())),
|
|
)
|
|
};
|
|
|
|
Self { x: x_enc, y: y_enc }
|
|
}
|
|
|
|
pub fn to_vector(self) -> Vector3f {
|
|
let mut v = Vector3f::default();
|
|
|
|
// Map [0, 65535] back to [-1, 1]
|
|
v[0] = -1.0 + 2.0 * (self.x as Float / 65535.0);
|
|
v[1] = -1.0 + 2.0 * (self.y as Float / 65535.0);
|
|
v[2] = 1.0 - (v.x().abs() + v.y().abs());
|
|
|
|
if v.z() < 0.0 {
|
|
let xo = v.x();
|
|
v[0] = (1.0 - v.y().abs()) * Self::sign(xo);
|
|
v[1] = (1.0 - xo.abs()) * Self::sign(v.y());
|
|
}
|
|
|
|
v.normalize()
|
|
}
|
|
|
|
#[inline]
|
|
pub fn sign(v: Float) -> Float {
|
|
1.0.copysign(v)
|
|
}
|
|
|
|
#[inline]
|
|
pub fn encode(f: Float) -> u16 {
|
|
(clamp((f + 1.0) / 2.0, 0.0, 1.0) * 65535.0).round() as u16
|
|
}
|
|
}
|
|
|
|
impl From<Vector3f> for OctahedralVector {
|
|
fn from(v: Vector3f) -> Self {
|
|
Self::new(v)
|
|
}
|
|
}
|
|
|
|
impl From<OctahedralVector> for Vector3f {
|
|
fn from(ov: OctahedralVector) -> Self {
|
|
ov.to_vector()
|
|
}
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Copy, Clone, Debug, Default, PartialEq)]
|
|
pub struct Frame {
|
|
pub x: Vector3f,
|
|
pub y: Vector3f,
|
|
pub z: Vector3f,
|
|
}
|
|
|
|
impl Frame {
|
|
pub fn new(x: Vector3f, z: Vector3f) -> Self {
|
|
Self {
|
|
x,
|
|
y: z.cross(x),
|
|
z,
|
|
}
|
|
}
|
|
|
|
pub fn from_x(x: Vector3f) -> Self {
|
|
let (y, z) = x.normalize().coordinate_system();
|
|
Self {
|
|
x: x.normalize(),
|
|
y,
|
|
z,
|
|
}
|
|
}
|
|
|
|
pub fn from_xz(x: Vector3f, z: Vector3f) -> Self {
|
|
Self {
|
|
x,
|
|
y: z.cross(x),
|
|
z,
|
|
}
|
|
}
|
|
|
|
pub fn from_xy(x: Vector3f, y: Vector3f) -> Self {
|
|
Self {
|
|
x,
|
|
y,
|
|
z: x.cross(y),
|
|
}
|
|
}
|
|
|
|
pub fn from_y(y: Vector3f) -> Self {
|
|
let (z, x) = y.normalize().coordinate_system();
|
|
Self {
|
|
x,
|
|
y: y.normalize(),
|
|
z,
|
|
}
|
|
}
|
|
|
|
pub fn from_z(z: Vector3f) -> Self {
|
|
let (x, y) = z.normalize().coordinate_system();
|
|
Self {
|
|
x,
|
|
y,
|
|
z: z.normalize(),
|
|
}
|
|
}
|
|
|
|
pub fn to_local(&self, v: Vector3f) -> Vector3f {
|
|
Vector3f::new(v.dot(self.x), v.dot(self.y), v.dot(self.z))
|
|
}
|
|
|
|
pub fn to_local_normal(&self, n: Normal3f) -> Normal3f {
|
|
let n: Vector3f = n.into();
|
|
Normal3f::new(n.dot(self.x), n.dot(self.y), n.dot(self.z))
|
|
}
|
|
|
|
pub fn from_local(&self, v: Vector3f) -> Vector3f {
|
|
self.x * v.x() + self.y * v.y() + self.z * v.z()
|
|
}
|
|
|
|
pub fn from_local_normal(&self, v: Normal3f) -> Normal3f {
|
|
Normal3f::from(self.x * v.x() + self.y * v.y() + self.z * v.z())
|
|
}
|
|
}
|