From 24d08042496dedd91565196267eb04102d07c3c5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 3 Jul 2020 14:05:34 +1000
Subject: [PATCH] AP_Math: added rotate() method to Vector2f

---
 libraries/AP_Math/vector2.cpp | 13 +++++++++++++
 libraries/AP_Math/vector2.h   |  3 +++
 2 files changed, 16 insertions(+)

diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index 8aa8cc30c6..8404fdedc2 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/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);
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index 5f3e27ef82..00fda1aaeb 100644
--- a/libraries/AP_Math/vector2.h
+++ b/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);