diff --git a/src/base/matrix.rs b/src/base/matrix.rs index 77f6c1c88..895713780 100644 --- a/src/base/matrix.rs +++ b/src/base/matrix.rs @@ -2123,6 +2123,72 @@ impl< } } +impl> + Matrix +{ + /// Calculate the right-handed angle between two vectors in radians. + /// + /// This function is very different to the [angle] function. + /// It always yields a positive result `x` between `0.0` and `2.0*PI` and is only applicable + /// for 2D vectors. + /// In the following example, we calculate the right-handed angle between the following two + /// vectors. + /// ```ignore + /// p + /// │ + /// │ + /// └─────── q + /// ``` + /// 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([0.0, 1.0]); + /// let q = Vector2::from([2.0, 0.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 two_pi = + (T::SimdRealField::one() + T::SimdRealField::one()) * T::SimdRealField::simd_pi(); + let perp = other.perp(self); + let dot = self.dot(other); + let res: T::SimdRealField = T::SimdRealField::simd_atan2(perp, dot) % two_pi.clone(); + use crate::SimdBool; + res.clone().simd_lt(T::SimdRealField::zero()).if_else( + || res.clone() + two_pi, + || res.clone() + ) + } +} + impl> Vector { /// Computes the matrix `M` such that for all vector `v` we have `M * v == self.cross(&v)`. #[inline]