AP_Math: added rotate() method to Vector2f

libraries/AP_Math/vector2.cpp

@@ -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);

libraries/AP_Math/vector2.h

@@ -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);