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

AP_Math: Extend vector2::angle(vector2) to distinguish parallel and antiparallel vectors

Browse Source 
There are a number of use cases where distingusihing antiparallel from parallel vectors is important
This commit is contained in:
Michael du Breuil 2017-03-01 16:22:01 -07:00 committed by Francisco Ferreira
parent a62d728933
commit c5c94949bf
2 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Math/vector2.cpp
View File

@ -131,9 +131,12 @@ float Vector2<T>::angle(const Vector2<T> &v2) const
return 0.0f;
}
float cosv = ((*this)*v2) / len;
if (fabsf(cosv) >= 1) {
if (cosv >= 1) {
return 0.0f;
}
if (cosv <= -1) {
return M_PI;
}
return acosf(cosv);
}

1
libraries/AP_Math/vector2.h
View File

@ -92,6 +92,7 @@ struct Vector2
T operator %(const Vector2<T> &v) const;
// computes the angle between this vector and another vector
// returns 0 if the vectors are parallel, and M_PI if they are antiparallel
float angle(const Vector2<T> &v2) const;
// computes the angle in radians between the origin and this vector