AP_Math: Add new method to extrapolate 3D vector given distance, yaw, pitch

libraries/AP_Math/vector3.cpp

@@ -418,6 +418,15 @@ Matrix3<T> Vector3<T>::mul_rowcol(const Vector3<T> &v2) const
                       v1.z * v2.x, v1.z * v2.y, v1.z * v2.z);
 }
 
+// extrapolate position given bearing and pitch (in degrees) and distance
+template <typename T>
+void Vector3<T>::offset_bearing(float bearing, float pitch, float distance)
+{
+    y += cosf(radians(pitch)) * sinf(radians(bearing)) * distance;
+    x += cosf(radians(pitch)) * cosf(radians(bearing)) * distance;
+    z += sinf(radians(pitch)) * distance;
+}
+
 // distance from the tip of this vector to a line segment specified by two vectors
 template <typename T>
 float Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const

libraries/AP_Math/vector3.h

@@ -244,6 +244,9 @@ public:
     // distance from the tip of this vector to a line segment specified by two vectors
     float distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const;
 
+    // extrapolate position given bearing and pitch (in degrees) and distance
+    void offset_bearing(float bearing, float pitch, float distance);
+    
     // given a position p1 and a velocity v1 produce a vector
     // perpendicular to v1 maximising distance from p1.  If p1 is the
     // zero vector the return from the function will always be the