Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_Math/AP_Math.cpp

This commit is contained in:
uncrustify 2012-08-16 23:20:14 -07:00 committed by Pat Hickey
parent 931ff231ef
commit 23341c6390
1 changed files with 40 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
libraries/AP_Math/AP_Math.cpp
View File

@ -5,16 +5,16 @@
// returned. // returned.
float safe_asin(float v) float safe_asin(float v)
{ {
if (isnan(v)) { if (isnan(v)) {
return 0.0; return 0.0;
} }
if (v >= 1.0) { if (v >= 1.0) {
return PI/2; return PI/2;
} }
if (v <= -1.0) { if (v <= -1.0) {
return -PI/2; return -PI/2;
} }
return asin(v); return asin(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
@ -24,11 +24,11 @@ float safe_asin(float v)
// real input should have been zero // real input should have been zero
float safe_sqrt(float v) float safe_sqrt(float v)
{ {
float ret = sqrt(v); float ret = sqrt(v);
if (isnan(ret)) { if (isnan(ret)) {
return 0; return 0;
} }
return ret; return ret;
} }
@ -39,31 +39,31 @@ float safe_sqrt(float v)
// optional 'found' parameter is for the test suite to ensure that it is. // optional 'found' parameter is for the test suite to ensure that it is.
enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found) enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found)
{ {
Vector3f tv1, tv2; Vector3f tv1, tv2;
enum Rotation r; enum Rotation r;
tv1(1,2,3); tv1(1,2,3);
tv1.rotate(r1); tv1.rotate(r1);
tv1.rotate(r2); tv1.rotate(r2);
for (r=ROTATION_NONE; r<ROTATION_MAX; for (r=ROTATION_NONE; r<ROTATION_MAX;
r = (enum Rotation)((uint8_t)r+1)) { r = (enum Rotation)((uint8_t)r+1)) {
Vector3f diff; Vector3f diff;
tv2(1,2,3); tv2(1,2,3);
tv2.rotate(r); tv2.rotate(r);
diff = tv1 - tv2; diff = tv1 - tv2;
if (diff.length() < 1.0e-6) { if (diff.length() < 1.0e-6) {
// we found a match // we found a match
if (found) { if (found) {
*found = true; *found = true;
} }
return r; return r;
} }
} }
// we found no matching rotation. Someone has edited the // we found no matching rotation. Someone has edited the
// rotations list and broken its completeness property ... // rotations list and broken its completeness property ...
if (found) { if (found) {
*found = false; *found = false;
} }
return ROTATION_NONE; return ROTATION_NONE;
} }