Browse Source

AP_Math: prevent a floating point exception

mission-4.1.18
Andrew Tridgell 11 years ago
parent
commit
7e5a491f14
  1. 10
      libraries/AP_Math/vector2.cpp

10
libraries/AP_Math/vector2.cpp

@ -125,7 +125,15 @@ bool Vector2<T>::operator !=(const Vector2<T> &v) const @@ -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

Loading…
Cancel
Save