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

rapier2d/utils/
orthonormal_basis.rs
