diff --git a/src/base/matrix.rs b/src/base/matrix.rs index 50599e34..f690071f 100644 --- a/src/base/matrix.rs +++ b/src/base/matrix.rs @@ -2111,6 +2111,57 @@ impl> + Matrix +{ + /// 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(&self, other: &Matrix) -> T + where + R2: Dim, + C2: Dim, + SB: RawStorage, + ShapeConstraint: DimEq + DimEq, + ShapeConstraint: SameNumberOfRows + + SameNumberOfColumns + + SameNumberOfRows + + SameNumberOfColumns, + { + 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> Vector { /// Computes the matrix `M` such that for all vector `v` we have `M * v == self.cross(&v)`. #[inline]