Projection friction impulses on an implicit cone instead of a pyramidal approximation.

1 files changed, 12 insertions, 0 deletions

diff --git a/src/utils.rs b/src/utils.rs
index e131b0a..4ed89f6 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -107,6 +107,8 @@ pub trait WBasis: Sized {
     type Basis;
     /// Computes the vectors which, when combined with `self`, form an orthonormal basis.
     fn orthonormal_basis(self) -> Self::Basis;
+    /// Computes a vector orthogonal to `self` with a unit length (if `self` has a unit length).
+    fn orthonormal_vector(self) -> Self;
 }
 
 impl<N: SimdRealField> WBasis for Vector2<N> {
@@ -114,6 +116,9 @@ impl<N: SimdRealField> WBasis for Vector2<N> {
     fn orthonormal_basis(self) -> [Vector2<N>; 1] {
         [Vector2::new(-self.y, self.x)]
     }
+    fn orthonormal_vector(self) -> Vector2<N> {
+        Vector2::new(-self.y, self.x)
+    }
 }
 
 impl<N: SimdRealField + WSign<N>> WBasis for Vector3<N> {
@@ -134,6 +139,13 @@ impl<N: SimdRealField + WSign<N>> WBasis for Vector3<N> {
             Vector3::new(b, sign + self.y * self.y * a, -self.y),
         ]
     }
+
+    fn orthonormal_vector(self) -> Vector3<N> {
+        let sign = self.z.copy_sign_to(N::one());
+        let a = -N::one() / (sign + self.z);
+        let b = self.x * self.y * a;
+        Vector3::new(b, sign + self.y * self.y * a, -self.y)
+    }
 }
 
 pub(crate) trait WVec: Sized {