From 7e5a491f14bdc164981a1b785e1b9de9324b8147 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 21 Apr 2014 15:23:25 +1000
Subject: [PATCH] AP_Math: prevent a floating point exception

---
 libraries/AP_Math/vector2.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index 214f4ac758..096d2e93ac 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -125,7 +125,15 @@ bool Vector2<T>::operator !=(const Vector2<T> &v) const
 template <typename T>
 float Vector2<T>::angle(const Vector2<T> &v2) const
 {
-    return acosf(((*this)*v2) / (this->length()*v2.length()));
+    float len = this->length() * v2.length();
+    if (len <= 0) {
+        return 0.0f;
+    }
+    float cosv = ((*this)*v2) / len;
+    if (fabsf(cosv) >= 1) {
+        return 0.0f;
+    }
+    return acosf(cosv);
 }
 
 // only define for float