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

AP_Math: fixed angle between two vector3s

This commit is contained in:
Andrew Tridgell 2016-01-01 12:26:52 +11:00
parent fcfd11ef53
commit 16e0a6d7b0
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/vector3.cpp
View File

@ -361,7 +361,15 @@ bool Vector3<T>::operator !=(const Vector3<T> &v) const
template <typename T>
float Vector3<T>::angle(const Vector3<T> &v2) const
{
return acosf((*this)*v2) / (float)((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);
}
// multiplication of transpose by a vector