Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Replace safe_asin() by template function

Browse Source
This commit is contained in:
dgrat 2016-04-16 11:59:08 +02:00 committed by Lucas De Marchi
parent 846b4927ec
commit 5deb0e8e03
2 changed files with 18 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Math/AP_Math.cpp
View File

@ -1,23 +1,26 @@
#include "AP_Math.h" #include "AP_Math.h"
#include <float.h> #include <float.h>
// a varient of asin() that checks the input ranges and ensures a template <class T>
// valid angle as output. If nan is given as input then zero is float safe_asin(const T v)
// returned.
float safe_asin(float v)
{ {
if (isnan(v)) { if (isnan(static_cast<float>(v))) {
return 0.0f; return 0.0f;
} }
if (v >= 1.0f) { if (v >= 1.0f) {
return M_PI/2; return static_cast<float>(M_PI_2);
} }
if (v <= -1.0f) { 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 // 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 // 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 // returned. The reasoning is that a negative number for sqrt() in our

9
libraries/AP_Math/AP_Math.h
View File

@ -30,8 +30,13 @@ static inline bool is_equal(const float fVal1, const float fVal2) { return fabsf
// is a float is zero // is a float is zero
static inline bool is_zero(const float fVal1) { return fabsf(fVal1) < FLT_EPSILON ? true : false; } 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. // a varient of sqrt() that always gives a valid answer.
float safe_sqrt(float v); float safe_sqrt(float v);