AP_Math: Vector2,3 get limit_length methods

libraries/AP_Math/vector2.cpp

@@ -32,6 +32,19 @@ float Vector2<T>::length(void) const
     return norm(x, y);
 }
 
+// limit vector to a given length. returns true if vector was limited
+template <typename T>
+bool Vector2<T>::limit_length(float max_length)
+{
+    const float len = length();
+    if ((len > max_length) && is_positive(len)) {
+        x *= (max_length / len);
+        y *= (max_length / len);
+        return true;
+    }
+    return false;
+}
+
 // dot product
 template <typename T>
 T Vector2<T>::operator *(const Vector2<T> &v) const

libraries/AP_Math/vector2.h

@@ -138,6 +138,9 @@ struct Vector2
     // gets the length of this vector
     float length(void) const;
 
+    // limit vector to a given length. returns true if vector was limited
+    bool limit_length(float max_length);
+
     // normalizes this vector
     void normalize();
 

libraries/AP_Math/vector3.cpp

@@ -303,6 +303,19 @@ float Vector3<T>::length(void) const
     return norm(x, y, z);
 }
 
+// limit xy component vector to a given length. returns true if vector was limited
+template <typename T>
+bool Vector3<T>::limit_length_xy(float max_length)
+{
+    const float length_xy = norm(x, y);
+    if ((length_xy > max_length) && is_positive(length_xy)) {
+        x *= (max_length / length_xy);
+        y *= (max_length / length_xy);
+        return true;
+    }
+    return false;
+}
+
 template <typename T>
 Vector3<T> &Vector3<T>::operator *=(const T num)
 {

libraries/AP_Math/vector3.h

@@ -195,6 +195,9 @@ public:
     // gets the length of this vector
     float length(void) const;
 
+    // limit xy component vector to a given length. returns true if vector was limited
+    bool limit_length_xy(float max_length);
+
     // normalizes this vector
     void normalize()
     {