From 5deb0e8e0352dc27e272049a355bc492a3c0dbf1 Mon Sep 17 00:00:00 2001
From: dgrat <dgdanielf@gmail.com>
Date: Sat, 16 Apr 2016 11:59:08 +0200
Subject: [PATCH] AP_Math: Replace safe_asin() by template function

---
 libraries/AP_Math/AP_Math.cpp | 19 +++++++++++--------
 libraries/AP_Math/AP_Math.h   |  9 +++++++--
 2 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index ccaee322ae..c9c8801d43 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -1,23 +1,26 @@
 #include "AP_Math.h"
 #include <float.h>
 
-// a varient of asin() that checks the input ranges and ensures a
-// valid angle as output. If nan is given as input then zero is
-// returned.
-float safe_asin(float v)
+template <class T>
+float safe_asin(const T v)
 {
-    if (isnan(v)) {
+    if (isnan(static_cast<float>(v))) {
         return 0.0f;
     }
     if (v >= 1.0f) {
-        return M_PI/2;
+        return static_cast<float>(M_PI_2);
     }
     if (v <= -1.0f) {
-        return -M_PI/2;
+        return static_cast<float>(-M_PI_2);
     }
-    return asinf(v);
+    return asinf(static_cast<float>(v));
 }
 
+template float safe_asin<int>(const int v);
+template float safe_asin<short>(const short v);
+template float safe_asin<float>(const float v);
+template float safe_asin<double>(const double v);
+
 // a varient of sqrt() that checks the input ranges and ensures a
 // valid value as output. If a negative number is given then 0 is
 // returned. The reasoning is that a negative number for sqrt() in our
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 0544883b12..2b52f9c19f 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -30,8 +30,13 @@ static inline bool is_equal(const float fVal1, const float fVal2) { return fabsf
 // is a float is zero
 static inline bool is_zero(const float fVal1) { return fabsf(fVal1) < FLT_EPSILON ? true : false; }
 
-// a varient of asin() that always gives a valid answer.
-float           safe_asin(float v);
+
+/*
+ * A variant of asin() that checks the input ranges and ensures a valid angle
+ * as output. If nan is given as input then zero is returned.
+ */
+template <class T>
+float safe_asin(const T v);
 
 // a varient of sqrt() that always gives a valid answer.
 float           safe_sqrt(float v);