VectorMath: Optimization by passing vector by reference instead of value

2 files changed, 22 insertions, 31 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
index 64a25aaba..470eb4e2c 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -38,6 +38,7 @@
  */
 
 #include "estimator_utilities.h"
+#include <algorithm>
 
 // Define EKF_DEBUG here to enable the debug print calls
 // if the macro is not set, these will be completely
@@ -104,17 +105,17 @@ void Mat3f::identity() {
     z.z = 1.0f;
 }
 
-Mat3f Mat3f::transpose(void) const
+Mat3f Mat3f::transpose() const
 {
     Mat3f ret = *this;
-    swap_var(ret.x.y, ret.y.x);
-    swap_var(ret.x.z, ret.z.x);
-    swap_var(ret.y.z, ret.z.y);
+    std::swap(ret.x.y, ret.y.x);
+    std::swap(ret.x.z, ret.z.x);
+    std::swap(ret.y.z, ret.z.y);
     return ret;
 }
 
 // overload + operator to provide a vector addition
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
+Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x + vecIn2.x;
@@ -124,7 +125,7 @@ Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
 }
 
 // overload - operator to provide a vector subtraction
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
+Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x - vecIn2.x;
@@ -134,7 +135,7 @@ Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
 }
 
 // overload * operator to provide a matrix vector product
-Vector3f operator*( Mat3f matIn, Vector3f vecIn)
+Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn)
 {
     Vector3f vecOut;
     vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
@@ -144,7 +145,7 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn)
 }
 
 // overload * operator to provide a matrix product
-Mat3f operator*( Mat3f matIn1, Mat3f matIn2)
+Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2)
 {
     Mat3f matOut;
     matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
@@ -163,7 +164,7 @@ Mat3f operator*( Mat3f matIn1, Mat3f matIn2)
 }
 
 // overload % operator to provide a vector cross product
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
+Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
@@ -173,7 +174,7 @@ Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
 }
 
 // overload * operator to provide a vector scaler product
-Vector3f operator*(Vector3f vecIn1, float sclIn1)
+Vector3f operator*(const Vector3f &vecIn1, const float sclIn1)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x * sclIn1;
@@ -183,7 +184,7 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1)
 }
 
 // overload * operator to provide a vector scaler product
-Vector3f operator*(float sclIn1, Vector3f vecIn1)
+Vector3f operator*(float sclIn1, const Vector3f &vecIn1)
 {
     Vector3f vecOut;
     vecOut.x = vecIn1.x * sclIn1;
@@ -192,13 +193,6 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1)
     return vecOut;
 }
 
-void swap_var(float &d1, float &d2)
-{
-    float tmp = d1;
-    d1 = d2;
-    d2 = tmp;
-}
-
 // overload / operator to provide a vector scalar division
 Vector3f operator/(const Vector3f &vec, const float scalar)
 {
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
index b2d790e27..c137209ff 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
@@ -52,7 +52,6 @@
 
 class Vector3f
 {
-private:
 public:
     float x;
     float y;
@@ -64,8 +63,8 @@ public:
     z(c)
     {}
 
-    float length(void) const;
-    void zero(void);
+    float length() const;
+    void zero();
 };
 
 class Mat3f
@@ -79,20 +78,18 @@ public:
     Mat3f();
 
     void identity();
-    Mat3f transpose(void) const;
+    Mat3f transpose() const;
 };
 
-Vector3f operator*(float sclIn1, Vector3f vecIn1);
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*( Mat3f matIn, Vector3f vecIn);
-Mat3f operator*( Mat3f matIn1, Mat3f matIn2);
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*(Vector3f vecIn1, float sclIn1);
+Vector3f operator*(const float sclIn1, const Vector3f &vecIn1);
+Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2);
+Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2);
+Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn);
+Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2);
+Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2);
+Vector3f operator*(const Vector3f &vecIn1, const float sclIn1);
 Vector3f operator/(const Vector3f &vec, const float scalar);
 
-void swap_var(float &d1, float &d2);
-
 enum GPS_FIX {
     GPS_FIX_NOFIX = 0,
     GPS_FIX_2D = 2,