rapier2d/utils/
orthonormal_basis.rs

1//! SimdBasis trait for computing orthonormal bases.
2
3#[cfg(feature = "dim3")]
4use crate::math::Real;
5use crate::math::Vector;
6#[cfg(not(target_arch = "spirv"))]
7use crate::utils::{CopySign, SimdRealCopy};
8#[cfg(not(target_arch = "spirv"))]
9use na::{Vector2, Vector3};
10
11/// Trait to compute the orthonormal basis of a vector.
12pub trait OrthonormalBasis: Sized {
13    /// The type of the array of orthonormal vectors.
14    type Basis;
15    /// Computes the vectors which, when combined with `self`, form an orthonormal basis.
16    fn orthonormal_basis(self) -> Self::Basis;
17    /// Computes a vector orthogonal to `self` with a unit length (if `self` has a unit length).
18    fn orthonormal_vector(self) -> Self;
19}
20
21#[cfg(not(target_arch = "spirv"))]
22impl<N: SimdRealCopy> OrthonormalBasis for Vector2<N> {
23    type Basis = [Vector2<N>; 1];
24    fn orthonormal_basis(self) -> [Vector2<N>; 1] {
25        [Vector2::new(-self.y, self.x)]
26    }
27    fn orthonormal_vector(self) -> Vector2<N> {
28        Vector2::new(-self.y, self.x)
29    }
30}
31
32#[cfg(not(target_arch = "spirv"))]
33impl<N: SimdRealCopy + CopySign<N>> OrthonormalBasis for Vector3<N> {
34    type Basis = [Vector3<N>; 2];
35    // Robust and branchless implementation from Pixar:
36    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
37    fn orthonormal_basis(self) -> [Vector3<N>; 2] {
38        let sign = self.z.copy_sign_to(N::one());
39        let a = -N::one() / (sign + self.z);
40        let b = self.x * self.y * a;
41
42        [
43            Vector3::new(
44                N::one() + sign * self.x * self.x * a,
45                sign * b,
46                -sign * self.x,
47            ),
48            Vector3::new(b, sign + self.y * self.y * a, -self.y),
49        ]
50    }
51
52    fn orthonormal_vector(self) -> Vector3<N> {
53        let sign = self.z.copy_sign_to(N::one());
54        let a = -N::one() / (sign + self.z);
55        let b = self.x * self.y * a;
56        Vector3::new(b, sign + self.y * self.y * a, -self.y)
57    }
58}
59
60// Glam implementations for concrete Vector type
61#[cfg(feature = "dim2")]
62impl OrthonormalBasis for Vector {
63    type Basis = [Vector; 1];
64    fn orthonormal_basis(self) -> [Vector; 1] {
65        [Vector::new(-self.y, self.x)]
66    }
67    fn orthonormal_vector(self) -> Vector {
68        Vector::new(-self.y, self.x)
69    }
70}
71
72#[cfg(feature = "dim3")]
73impl OrthonormalBasis for Vector {
74    type Basis = [Vector; 2];
75    // Robust and branchless implementation from Pixar:
76    // https://graphics.pixar.com/library/OrthonormalB/paper.pdf
77    fn orthonormal_basis(self) -> [Vector; 2] {
78        let sign = Real::copysign(1.0, self.z);
79        let a = -1.0 / (sign + self.z);
80        let b = self.x * self.y * a;
81
82        [
83            Vector::new(1.0 + sign * self.x * self.x * a, sign * b, -sign * self.x),
84            Vector::new(b, sign + self.y * self.y * a, -self.y),
85        ]
86    }
87
88    fn orthonormal_vector(self) -> Vector {
89        let sign = Real::copysign(1.0, self.z);
90        let a = -1.0 / (sign + self.z);
91        let b = self.x * self.y * a;
92        Vector::new(b, sign + self.y * self.y * a, -self.y)
93    }
94}
rapier2d/utils/orthonormal_basis.rs

rapier2d/utils/
orthonormal_basis.rs
