AP_Math: allow selection of inline version of vector operations for performance.

libraries/AP_Math/SCurve.cpp

@@ -545,7 +545,7 @@ void SCurve::move_to_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vecto
 void SCurve::move_from_time_pos_vel_accel(float time_now, Vector3f &pos, Vector3f &vel, Vector3f &accel)
 {
     float scurve_P1 = 0.0f;
-    float scurve_V1, scurve_A1, scurve_J1;
+    float scurve_V1 = 0.0f, scurve_A1 = 0.0f, scurve_J1 = 0.0f;
     get_jerk_accel_vel_pos_at_time(time_now, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
     pos += delta_unit * scurve_P1;
     vel += delta_unit * scurve_V1;

libraries/AP_Math/vector3.h

@@ -315,6 +315,83 @@ public:
     static bool segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point);
 };
 
+// The creation of temporary vector objects as return types creates a significant overhead in certain hot 
+// code paths. This allows callers to select the inline versions where profiling shows a significant benefit
+#if defined(AP_INLINE_VECTOR_OPS) && !defined(HAL_DEBUG_BUILD)
+
+// vector cross product
+template <typename T>
+inline Vector3<T> Vector3<T>::operator %(const Vector3<T> &v) const
+{
+    return Vector3<T>(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
+}
+
+// dot product
+template <typename T>
+inline T Vector3<T>::operator *(const Vector3<T> &v) const
+{
+    return x*v.x + y*v.y + z*v.z;
+}
+
+template <typename T>
+inline Vector3<T> &Vector3<T>::operator *=(const T num)
+{
+    x*=num; y*=num; z*=num;
+    return *this;
+}
+
+template <typename T>
+inline Vector3<T> &Vector3<T>::operator /=(const T num)
+{
+    x /= num; y /= num; z /= num;
+    return *this;
+}
+
+template <typename T>
+inline Vector3<T> &Vector3<T>::operator -=(const Vector3<T> &v)
+{
+    x -= v.x; y -= v.y; z -= v.z;
+    return *this;
+}
+
+template <typename T>
+inline Vector3<T> &Vector3<T>::operator +=(const Vector3<T> &v)
+{
+    x+=v.x; y+=v.y; z+=v.z;
+    return *this;
+}
+
+template <typename T>
+inline Vector3<T> Vector3<T>::operator /(const T num) const
+{
+    return Vector3<T>(x/num, y/num, z/num);
+}
+
+template <typename T>
+inline Vector3<T> Vector3<T>::operator *(const T num) const
+{
+    return Vector3<T>(x*num, y*num, z*num);
+}
+
+template <typename T>
+inline Vector3<T> Vector3<T>::operator -(const Vector3<T> &v) const
+{
+    return Vector3<T>(x-v.x, y-v.y, z-v.z);
+}
+
+template <typename T>
+inline Vector3<T> Vector3<T>::operator +(const Vector3<T> &v) const
+{
+    return Vector3<T>(x+v.x, y+v.y, z+v.z);
+}
+
+template <typename T>
+inline Vector3<T> Vector3<T>::operator -(void) const
+{
+    return Vector3<T>(-x,-y,-z);
+}
+#endif
+
 typedef Vector3<int16_t>                Vector3i;
 typedef Vector3<uint16_t>               Vector3ui;
 typedef Vector3<int32_t>                Vector3l;