AP_Math: Implement quaternion vector rotation operator

Overloads operator*() with semantics of applying a rotation to a vector, in
common with usage in Eigen/OpenGL. This implementation carries out 30
operations, compared to 58 operations for the q*v*q.inverse() formula

libraries/AP_Math/quaternion.cpp

@@ -620,6 +620,33 @@ Quaternion Quaternion::operator*(const Quaternion &v) const
     return ret;
 }
 
+// Optimized quaternion rotation operator, equivalent to converting
+// (*this) to a rotation matrix then multiplying it to the argument `v`.
+//
+// 15 multiplies and 15 add / subtracts. Caches 3 floats
+Vector3f Quaternion::operator*(const Vector3f &v) const
+{
+    // This uses the formula
+    //
+    //    v2 = v1 + 2 q1 * qv x v1 + 2 qv x qv x v1
+    //
+    // where "x" is the cross product (explicitly inlined for performance below), 
+    // "q1" is the scalar part and "qv" is the vector part of this quaternion
+
+    Vector3f ret = v;
+
+    // Compute and cache "qv x v1"
+    float uv[] = {q3 * v.z - q4 * v.y, q4 * v.x - q2 * v.z, q2 * v.y - q3 * v.x};
+
+    uv[0] += uv[0];
+    uv[1] += uv[1];
+    uv[2] += uv[2];
+    ret.x += q1 * uv[0] + q3 * uv[2] - q4 * uv[1];
+    ret.y += q1 * uv[1] + q4 * uv[0] - q2 * uv[2];
+    ret.z += q1 * uv[2] + q2 * uv[1] - q3 * uv[0];
+    return ret;
+}
+
 Quaternion &Quaternion::operator*=(const Quaternion &v)
 {
     const float w1 = q1;

libraries/AP_Math/quaternion.h

@@ -164,6 +164,8 @@ public:
     }
 
     Quaternion operator*(const Quaternion &v) const;
+
+    Vector3f operator*(const Vector3f &v) const;
     Quaternion &operator*=(const Quaternion &v);
     Quaternion operator/(const Quaternion &v) const;