From c5c94949bfc4b5b1d13a210dfcc5aac903969594 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Wed, 1 Mar 2017 16:22:01 -0700
Subject: [PATCH] AP_Math: Extend vector2::angle(vector2) to distinguish
 parallel and antiparallel vectors

There are a number of use cases where distingusihing antiparallel from parallel vectors is important
---
 libraries/AP_Math/vector2.cpp | 5 ++++-
 libraries/AP_Math/vector2.h   | 1 +
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index 0825499d00..9145d8255d 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -131,9 +131,12 @@ float Vector2<T>::angle(const Vector2<T> &v2) const
         return 0.0f;
     }
     float cosv = ((*this)*v2) / len;
-    if (fabsf(cosv) >= 1) {
+    if (cosv >= 1) {
         return 0.0f;
     }
+    if (cosv <= -1) {
+        return M_PI;
+    }
     return acosf(cosv);
 }
 
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index d2284d6b68..dc3838115a 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -92,6 +92,7 @@ struct Vector2
     T operator %(const Vector2<T> &v) const;
 
     // computes the angle between this vector and another vector
+    // returns 0 if the vectors are parallel, and M_PI if they are antiparallel
     float angle(const Vector2<T> &v2) const;
 
     // computes the angle in radians between the origin and this vector