add new function to calculate the right-handed angle between two 2d

vectors

src/base/matrix.rs

@@ -2111,6 +2111,57 @@ impl<T: Scalar + ClosedAdd + ClosedSub + ClosedMul, R: Dim, C: Dim, S: RawStorag
     }
 }
 
+impl<T: crate::SimdRealField + num::Euclid + ClosedMul, R: Dim, C: Dim, S: RawStorage<T, R, C>>
+    Matrix<T, R, C, S>
+{
+    /// Calculate the right-handed angle between two vectors in radians.
+    ///
+    /// This function always yields a positive result `x` between `0.0` and `2.0*PI`.
+    /// Note that the order in which the arguments are executed plays an important role.
+    /// Furthermore, we can see that the sum of both results `PI/2.0` and `3.0*PI/2.0` add up to a
+    /// total of `2.0*PI`.
+    /// ```
+    /// # use nalgebra::Vector2;
+    /// let p = Vector2::from([1.0, 0.0]);
+    /// let q = Vector2::from([0.0, -1.0]);
+    ///
+    /// let abs_diff1 = (p.angle_right_handed(&q) - std::f64::consts::FRAC_PI_2).abs();
+    /// let abs_diff2 = (q.angle_right_handed(&p) - 3.0 * std::f64::consts::FRAC_PI_2).abs();
+    /// assert!(abs_diff1 < 1e-10);
+    /// assert!(abs_diff2 < 1e-10);
+    /// ```
+    pub fn angle_right_handed<R2, C2, SB>(&self, other: &Matrix<T, R2, C2, SB>) -> T
+    where
+        R2: Dim,
+        C2: Dim,
+        SB: RawStorage<T, R2, C2>,
+        ShapeConstraint: DimEq<R, R2> + DimEq<C, C2>,
+        ShapeConstraint: SameNumberOfRows<R, U2>
+            + SameNumberOfColumns<C, U1>
+            + SameNumberOfRows<R2, U2>
+            + SameNumberOfColumns<C2, U1>,
+    {
+        let shape = self.shape();
+        assert_eq!(
+            shape,
+            other.shape(),
+            "2D vector angle_right_handed dimension mismatch."
+        );
+        assert_eq!(
+            shape,
+            (2, 1),
+            "2D angle_right_handed requires (2, 1) vectors {:?}",
+            shape
+        );
+
+        let perp = -self.perp(other);
+        let dot = self.dot(other);
+        T::SimdRealField::simd_atan2(perp, dot).rem_euclid(
+            &((T::SimdRealField::one() + T::SimdRealField::one()) * T::SimdRealField::simd_pi()),
+        )
+    }
+}
+
 impl<T: Scalar + Field, S: RawStorage<T, U3>> Vector<T, U3, S> {
     /// Computes the matrix `M` such that for all vector `v` we have `M * v == self.cross(&v)`.
     #[inline]