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

AP_Math: prevent a floating point exception

This commit is contained in:
Andrew Tridgell 2014-04-21 15:23:25 +10:00
parent 4d24a86088
commit 7e5a491f14
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Math/vector2.cpp
View File

@ -125,7 +125,15 @@ bool Vector2<T>::operator !=(const Vector2<T> &v) const
template <typename T> template <typename T>
float Vector2<T>::angle(const Vector2<T> &v2) const 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 // only define for float