Repository: bitshifter/glam-rs
Pull request #50 (2020-04-14 00:58:11)

Add angle between two vectors function

aloucks: @bitshifter 

I tried out a few different implementations and benchmarked them. I noticed that using your `scalar_acos` seemed to have the most impact and hunted down an `atan2` approximation (at the very bottom, which happens to be the fastest). Note that all of the implementations using approximations cause the unit test to fail due to loss of precision. 

```
bench_binop!(
    vec3_angle,
    "vec3 angle",
    op => angle,
    from1 => random_vec3,
    from2 => random_vec3
);
```

## Implementation:
```rust
#[inline]
pub fn angle(self, other: Self) -> f32 {
    f32::atan2(self.cross(other).length(), self.dot(other))
}
```
```
vec3 angle              time:   [16.382 ns 16.410 ns 16.441 ns]
```

## Implementation:
```rust
#[inline]
pub fn angle(self, other: Self) -> f32 {
    f32::acos(self.dot(other) / (self.length() * other.length()))
}
```
```
vec3 angle              time:   [12.235 ns 12.297 ns 12.360 ns]
```

## Implementation:
```rust
#[inline]
pub fn angle(self, other: Self) -> f32 {
    crate::f32::funcs::scalar_acos(self.dot(other) / (self.length() * other.length()))
}
```
```
vec3 angle              time:   [8.5161 ns 8.5244 ns 8.5329 ns]
```

## Implementation
```rust
#[inline]
pub fn angle(self, other: Self) -> f32 {
    f32::acos(self.dot(other) / (self.dot(self) * other.dot(other)).sqrt())
}
```
```
vec3 angle              time:   [12.529 ns 12.593 ns 12.659 ns]
```

## Implementation
```rust
#[inline]
pub fn angle(self, other: Self) -> f32 {
    crate::f32::funcs::scalar_acos(self.dot(other) / (self.dot(self) * other.dot(other)).sqrt())
}
```
```
vec3 angle              time:   [8.3654 ns 8.3754 ns 8.3864 ns]
```

## Implementation
```rust
#[inline]
pub fn angle(self, other: Self) -> f32 {
    // From DirectXMath XMVector3AngleBetweenVectors.
    let cos_angle = self.dot(other) * self.length_reciprocal() * other.length_reciprocal();
    crate::f32::funcs::scalar_acos(cos_angle)
}
```
```
vec3 angle              time:   [9.4535 ns 9.4710 ns 9.4897 ns]
```

## Implementation

```rust
#[inline]
pub fn angle(self, other: Self) -> f32 {
    // https://www.dsprelated.com/showarticle/1052.php

    fn approx_atan(z: f32) -> f32 {
        const N1: f32 = 0.97239411;
        const N2: f32 = -0.19194795;
        (N1 + N2 * z * z) * z
    }
    
    fn approx_atan2(y: f32, x: f32) -> f32 {
        use std::f32::consts::{PI, FRAC_PI_2};
        if x != 0.0 {
            if x.abs() > y.abs() {
                let z = y / x;
                if x > 0.0 {
                    approx_atan(z)
                } else if y >= 0.0 {
                    approx_atan(z) + PI
                } else {
                    approx_atan(z) - PI
                }
            } else {
                let z = x / y;
                if y > 0.0 {
                    -approx_atan(z) + FRAC_PI_2
                } else {
                    approx_atan(z) - FRAC_PI_2
                }
            }
        } else if y > 0.0 {
            FRAC_PI_2
        } else if y < 0.0 {
            -FRAC_PI_2
        } else {
            0.0
        }
    }

    approx_atan2(self.cross(other).length(), self.dot(other))
}
```
```
vec3 angle              time:   [6.4230 ns 6.4355 ns 6.4500 ns]
```
aloucks: @bitshifter I've updated this to use the fast approximation of `atan2` described here: https://www.dsprelated.com/showarticle/1052.php

It has a max error of just under `0.005` radians. If your `scalar_acos` function has similar accuracy, I think this should be ok. Otherwise, we probably don't want to use the approximation.  
bitshifter: Hey, thanks for doing all that investigation.

I'm a little bit concerned about the error of the atan2 approximation. I initially had my own sin_cos implementation in glam but I found that the error grew as the angle moved away from [-2PI, +2PI], so I'm rethinking that one. Same for atan2, I don't want to people to rely on this and end up getting unexpected results.

If you want to pursue the `atan2` approximation you could add a test similar to the test for `scalar_acos` in `src/f32/funcs.rs`. That tests compares `scalar_acos` with the `std::f32::acos` across a range of values to check the error is under `1-e6`. If `scalar_atan2` is within the same error range then I'm OK with it. Otherwise I think one of the `acos` versions would be OK. I'd prefer a slightly more explicit name like `angle_between`. It would be good to comment that the method doesn't require unit length vectors and will perform a `sqrt`. If users know their input vectors are unit length they might want to write their own function without the `sqrt`, or something to that effect.
aloucks: @bitshifter I renamed the methods, updated the doc comments, and switched to this `acos` implementation:

```rust
/// Returns the angle between two vectors, in radians.
///
/// The vectors do not need to be unit length, but this function does
/// perform a `sqrt`.
#[inline]
pub fn angle_between(self, other: Self) -> f32 {
    crate::f32::funcs::scalar_acos(self.dot(other) / (self.dot(self) * other.dot(other)).sqrt())
}
```
```
vec3 angle              time:   [8.3654 ns 8.3754 ns 8.3864 ns]
```

Patch:
diff --git a/benches/vec2.rs b/benches/vec2.rs
index b156977..6c93059 100644
--- a/benches/vec2.rs
+++ b/benches/vec2.rs
@@ -18,6 +18,14 @@ bench_binop!(
     from2 => random_vec2
 );
 
-criterion_group!(benches, vec2_euler, mat2_mul_vec2);
+bench_binop!(
+    vec2_angle_between,
+    "vec2 angle_between",
+    op => angle_between,
+    from1 => random_vec2,
+    from2 => random_vec2
+);
+
+criterion_group!(benches, vec2_euler, mat2_mul_vec2, vec2_angle_between);
 
 criterion_main!(benches);
diff --git a/benches/vec3.rs b/benches/vec3.rs
index e10d214..4f26b23 100644
--- a/benches/vec3.rs
+++ b/benches/vec3.rs
@@ -75,10 +75,19 @@ bench_func!(
 
 euler!(vec3_euler, "vec3 euler", ty => Vec3, storage => Vec3, zero => Vec3::zero(), rand => random_vec3);
 
+bench_binop!(
+    vec3_angle_between,
+    "vec3 angle_between",
+    op => angle_between,
+    from1 => random_vec3,
+    from2 => random_vec3
+);
+
 criterion_group!(
     benches,
     quat_mul_vec3,
     mat3_mul_vec3,
+    vec3_angle_between,
     vec3_euler,
     vec3_to_rgb,
     vec3_to_array_accessors,
diff --git a/src/f32/vec2.rs b/src/f32/vec2.rs
index b9a8601..063ea17 100644
--- a/src/f32/vec2.rs
+++ b/src/f32/vec2.rs
@@ -337,6 +337,29 @@ impl Vec2 {
     pub fn ceil(self) -> Self {
         Self(self.0.ceil(), self.1.ceil())
     }
+
+    /// The perpendicular dot product of the vector and `other`.
+    #[inline]
+    pub fn perp_dot(self, other: Vec2) -> f32 {
+        (self.0 * other.1) - (self.1 * other.0)
+    }
+
+    /// Returns the angle between two vectors, in radians.
+    ///
+    /// The vectors do not need to be unit length, but this function does
+    /// perform a `sqrt`.
+    #[inline]
+    pub fn angle_between(self, other: Self) -> f32 {
+        let angle = crate::f32::funcs::scalar_acos(
+            self.dot(other) / (self.dot(self) * other.dot(other)).sqrt(),
+        );
+
+        if self.perp_dot(other) < 0.0 {
+            -angle
+        } else {
+            angle
+        }
+    }
 }
 
 impl fmt::Display for Vec2 {
diff --git a/src/f32/vec3.rs b/src/f32/vec3.rs
index cb0f7ed..da1f02a 100644
--- a/src/f32/vec3.rs
+++ b/src/f32/vec3.rs
@@ -790,6 +790,15 @@ impl Vec3 {
     pub fn abs_diff_eq(self, other: Self, max_abs_diff: f32) -> bool {
         abs_diff_eq!(self, other, max_abs_diff)
     }
+
+    /// Returns the angle between two vectors, in radians.
+    ///
+    /// The vectors do not need to be unit length, but this function does
+    /// perform a `sqrt`.
+    #[inline]
+    pub fn angle_between(self, other: Self) -> f32 {
+        crate::f32::funcs::scalar_acos(self.dot(other) / (self.dot(self) * other.dot(other)).sqrt())
+    }
 }
 
 impl AsRef<[f32; 3]> for Vec3 {


Test patch:
diff --git a/tests/vec2.rs b/tests/vec2.rs
index c2cd646..9e8daba 100644
--- a/tests/vec2.rs
+++ b/tests/vec2.rs
@@ -1,3 +1,5 @@
+mod support;
+
 use glam::*;
 #[cfg(feature = "rand")]
 use rand::{Rng, SeedableRng};
@@ -387,3 +389,15 @@ fn test_vec2_serde() {
     let deserialized = serde_json::from_str::<Vec2>("[1.0,2.0,3.0]");
     assert!(deserialized.is_err());
 }
+
+#[test]
+fn test_vec2_angle_between() {
+    let angle = Vec2::new(1.0, 0.0).angle_between(Vec2::new(0.0, 1.0));
+    assert_approx_eq!(f32::consts::FRAC_PI_2, angle, 1e-6);
+
+    let angle = Vec2::new(10.0, 0.0).angle_between(Vec2::new(0.0, 5.0));
+    assert_approx_eq!(f32::consts::FRAC_PI_2, angle, 1e-6);
+
+    let angle = Vec2::new(-1.0, 0.0).angle_between(Vec2::new(0.0, 1.0));
+    assert_approx_eq!(-f32::consts::FRAC_PI_2, angle, 1e-6);
+}
diff --git a/tests/vec3.rs b/tests/vec3.rs
index 5b16862..4548aa9 100644
--- a/tests/vec3.rs
+++ b/tests/vec3.rs
@@ -449,3 +449,15 @@ fn test_vec3_serde() {
     let deserialized = serde_json::from_str::<Vec3>("[1.0,2.0,3.0,4.0]");
     assert!(deserialized.is_err());
 }
+
+#[test]
+fn test_vec3_angle_between() {
+    let angle = Vec3::new(1.0, 0.0, 1.0).angle_between(Vec3::new(1.0, 1.0, 0.0));
+    assert_approx_eq!(f32::consts::FRAC_PI_3, angle, 1e-6);
+
+    let angle = Vec3::new(10.0, 0.0, 10.0).angle_between(Vec3::new(5.0, 5.0, 0.0));
+    assert_approx_eq!(f32::consts::FRAC_PI_3, angle, 1e-6);
+
+    let angle = Vec3::new(-1.0, 0.0, -1.0).angle_between(Vec3::new(1.0, -1.0, 0.0));
+    assert_approx_eq!(2.0 * f32::consts::FRAC_PI_3, angle, 1e-6);
+}