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

vectors
This commit is contained in:
Jonas Pleyer 2024-06-13 16:18:13 +02:00
parent c23807ac5d
commit a1f34d5926

View File

@ -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]