From 475f9a594b1088e8cb34b2d7c6f550de5a0193d7 Mon Sep 17 00:00:00 2001
From: Roman Bapst <romanbapst@yahoo.de>
Date: Tue, 23 Sep 2014 16:55:19 +0200
Subject: [PATCH] Adapted math library for use of PX4 and ROS as shared
 library. First version, it works but some things might still be ugly

---
 src/lib/mathlib/math/Limits.hpp     |  5 ++++-
 src/lib/mathlib/math/Matrix.hpp     | 20 ++++++++++++++++++++
 src/lib/mathlib/math/Quaternion.hpp |  4 ++++
 src/lib/mathlib/math/Vector.hpp     | 11 +++++++++++
 4 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index fb778dd669..713cb51b56 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -39,7 +39,10 @@
 
 #pragma once
 
+#ifdef CONFIG_ARCH_ARM
 #include <nuttx/config.h>
+#endif
+
 #include <stdint.h>
 
 namespace math {
@@ -84,4 +87,4 @@ float __EXPORT degrees(float radians);
 
 double __EXPORT degrees(double radians);
 
-}
\ No newline at end of file
+}
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index ca931e2daf..8fb3caa0d1 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -44,7 +44,15 @@
 #define MATRIX_HPP
 
 #include <stdio.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <eigen/Eigen/Dense>
+#define M_PI_2_F 1.570769
+#endif
 
 namespace math
 {
@@ -65,7 +73,11 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
+	#else
+	eigen_matrix_instance arm_mat;
+	#endif
 
 	/**
 	 * trivial ctor
@@ -352,8 +364,16 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
+		#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+		#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::VectorXf Product = Me * Vec;
+		Vector<M> res(Product.data());
+		#endif
 		return res;
 	}
 };
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 21d05c7ef1..d8acc44434 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -44,7 +44,11 @@
 #define QUATERNION_HPP
 
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#endif
+
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 0ddf776155..51a84d27eb 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -45,7 +45,13 @@
 
 #include <stdio.h>
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <eigen/Eigen/Dense>
+#endif
 
 namespace math
 {
@@ -65,7 +71,12 @@ public:
 	/**
 	 * struct for using arm_math functions, represents column vector
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_col;
+	#else
+	eigen_matrix_instance arm_col;
+	#endif
+
 
 	/**
 	 * trivial ctor