Adapted for shared library use with ROS

1 files changed, 68 insertions, 46 deletions

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 8fb3caa0d..34fc4604a 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -54,10 +54,9 @@
 #define M_PI_2_F 1.570769
 #endif
 
-namespace math
-{
+namespace math {
 
-template <unsigned int M, unsigned int N>
+template<unsigned int M, unsigned int N>
 class __EXPORT Matrix;
 
 // MxN matrix with float elements
@@ -73,19 +72,19 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
-	#ifdef CONFIG_ARCH_ARM
+#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
-	#else
+#else
 	eigen_matrix_instance arm_mat;
-	#endif
+#endif
 
 	/**
 	 * trivial ctor
 	 * Initializes the elements to zero.
 	 */
-	MatrixBase() : 
-		data{},
-		arm_mat{M, N, &data[0][0]}
+	MatrixBase() :
+	data {},
+	arm_mat {M, N, &data[0][0]}
 	{
 	}
 
@@ -95,19 +94,19 @@ public:
 	 * copyt ctor
 	 */
 	MatrixBase(const MatrixBase<M, N> &m) :
-		arm_mat{M, N, &data[0][0]}
+	arm_mat {M, N, &data[0][0]}
 	{
 		memcpy(data, m.data, sizeof(data));
 	}
 
 	MatrixBase(const float *d) :
-		arm_mat{M, N, &data[0][0]}
+	arm_mat {M, N, &data[0][0]}
 	{
 		memcpy(data, d, sizeof(data));
 	}
 
-	MatrixBase(const float d[M][N]) : 
-		arm_mat{M, N, &data[0][0]}
+	MatrixBase(const float d[M][N]) :
+	arm_mat {M, N, &data[0][0]}
 	{
 		memcpy(data, d, sizeof(data));
 	}
@@ -159,9 +158,9 @@ public:
 	 */
 	bool operator ==(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
-					return false;
+		for (unsigned int j = 0; j < N; j++)
+		if (data[i][j] != m.data[i][j])
+		return false;
 
 		return true;
 	}
@@ -171,9 +170,9 @@ public:
 	 */
 	bool operator !=(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
-					return true;
+		for (unsigned int j = 0; j < N; j++)
+		if (data[i][j] != m.data[i][j])
+		return true;
 
 		return false;
 	}
@@ -193,8 +192,8 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				res.data[i][j] = -data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		res.data[i][j] = -data[i][j];
 
 		return res;
 	}
@@ -206,16 +205,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				res.data[i][j] = data[i][j] + m.data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		res.data[i][j] = data[i][j] + m.data[i][j];
 
 		return res;
 	}
 
 	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				data[i][j] += m.data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		data[i][j] += m.data[i][j];
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -227,16 +226,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				res.data[i][j] = data[i][j] - m.data[i][j];
+		for (unsigned int j = 0; j < N; j++)
+		res.data[i][j] = data[i][j] - m.data[i][j];
 
 		return res;
 	}
 
 	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
-				data[i][j] -= m.data[i][j];
+		for (unsigned int j = 0; j < M; j++)
+		data[i][j] -= m.data[i][j];
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -248,16 +247,17 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				res.data[i][j] = data[i][j] * num;
+		for (unsigned int j = 0; j < N; j++)
+		res.data[i][j] = data[i][j] * num;
 
 		return res;
+
 	}
 
 	Matrix<M, N> &operator *=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				data[i][j] *= num;
+		for (unsigned int j = 0; j < N; j++)
+		data[i][j] *= num;
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -266,16 +266,16 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				res[i][j] = data[i][j] / num;
+		for (unsigned int j = 0; j < N; j++)
+		res[i][j] = data[i][j] / num;
 
 		return res;
 	}
 
 	Matrix<M, N> &operator /=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
-				data[i][j] /= num;
+		for (unsigned int j = 0; j < N; j++)
+		data[i][j] /= num;
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -285,27 +285,49 @@ public:
 	 */
 	template <unsigned int P>
 	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, P> res;
 		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::Matrix<float,N,P,Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float,N,P,Eigen::RowMajor> >(m.arm_mat.pData);
+		Eigen::Matrix<float,M,P,Eigen::RowMajor> Product = Me * Him;
+		Matrix<M, P> res(Product.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<N, M> res;
 		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float,N,M,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,N,M,Eigen::RowMajor> >(this->arm_mat.pData);
+		Me.transposeInPlace();
+		Matrix<N, M> res(Me.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * invert the matrix
 	 */
 	Matrix<M, N> inversed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, N> res;
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> MyInverse = Me.inverse();//not sure if A = A.inverse() is a good idea
+		Matrix<M, N> res(MyInverse.data());
+		return res;
+#endif
 	}
 
 	/**
@@ -323,7 +345,7 @@ public:
 		unsigned int n = (M < N) ? M : N;
 
 		for (unsigned int i = 0; i < n; i++)
-			data[i][i] = 1;
+		data[i][i] = 1;
 	}
 
 	void print(void) {
@@ -331,7 +353,7 @@ public:
 			printf("[ ");
 
 			for (unsigned int j = 0; j < N; j++)
-				printf("%.3f\t", data[i][j]);
+			printf("%.3f\t", data[i][j]);
 
 			printf(" ]\n");
 		}
@@ -364,16 +386,16 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
-		#ifdef CONFIG_ARCH_ARM
+#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
-		#else
+#else
 		//probably nicer if this could go into a function like "eigen_mat_mult" or so
-		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me  = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
-		Eigen::VectorXf Vec  = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N);
+		Eigen::Matrix<float,M,N,Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float,M,N,Eigen::RowMajor> >(this->arm_mat.pData);
+		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData,N);
 		Eigen::VectorXf Product = Me * Vec;
 		Vector<M> res(Product.data());
-		#endif
+#endif
 		return res;
 	}
 };
@@ -405,8 +427,8 @@ public:
 	 */
 	Vector<3> operator *(const Vector<3> &v) const {
 		Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
-			      data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
-			      data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
+				data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
+				data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
 		return res;
 	}