dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VectorMath: Optimization by passing vector by reference instead of value

This commit is contained in:
Johan Jansen 2015-03-08 12:26:59 +01:00
parent 34f6cf9eb6
commit 1dc7d4905c
2 changed files with 22 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
View File

@ -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)
{

23
src/modules/ekf_att_pos_estimator/estimator_utilities.h
View File

@ -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,