Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: added rotate() method to Vector2f

Browse Source
This commit is contained in:
Andrew Tridgell 2020-07-03 14:05:34 +10:00 committed by Peter Barker
parent 6c9d756f11
commit 24d0804249
2 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Math/vector2.cpp
View File

@ -421,6 +421,18 @@ float Vector2<T>::closest_distance_between_radial_and_point(const Vector2<T> &w,
return sqrtf(closest_distance_between_radial_and_point_squared(w,p));
}
// rotate vector by angle in radians
template <typename T>
void Vector2<T>::rotate(float angle_rad)
{
const float cs = cosf(angle_rad);
const float sn = sinf(angle_rad);
float rx = x * cs - y * sn;
float ry = x * sn + y * cs;
x = rx;
y = ry;
}
// only define for float
template float Vector2<float>::length_squared(void) const;
template float Vector2<float>::length(void) const;
@ -454,6 +466,7 @@ template float Vector2<float>::closest_distance_between_line_and_point(const Vec
template float Vector2<float>::closest_distance_between_line_and_point_squared(const Vector2<float> &w1, const Vector2<float> &w2, const Vector2<float> &p);
template float Vector2<float>::closest_distance_between_lines_squared(const Vector2<float> &a1,const Vector2<float> &a2,const Vector2<float> &b1,const Vector2<float> &b2);
template Vector2<float> Vector2<float>::projected(const Vector2<float> &v);
template void Vector2<float>::rotate(float angle);
template void Vector2<float>::reflect(const Vector2<float> &n);

3
libraries/AP_Math/vector2.h
View File

@ -152,6 +152,9 @@ struct Vector2
// adjust position by a given bearing (in degrees) and distance
void offset_bearing(float bearing, float distance);
// rotate vector by angle in radians
void rotate(float angle_rad);
// given a position p1 and a velocity v1 produce a vector
// perpendicular to v1 maximising distance from p1
static Vector2<T> perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1);