mathlib: fixes and improvements, WIP

4 files changed, 157 insertions, 63 deletions

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f05360a19..1849f58be 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -34,9 +34,9 @@
  ****************************************************************************/
 
 /**
- * @file Matrix3.hpp
+ * @file Matrix.hpp
  *
- * 3x3 Matrix
+ * Generic Matrix
  */
 
 #ifndef MATRIX_HPP
@@ -204,6 +204,24 @@ public:
 	}
 
 	/**
+	 * transpose the matrix
+	 */
+	Matrix<N, M> transposed(void) const {
+        Matrix<N, M> res;
+        arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
+        return res;
+	}
+
+	/**
+	 * invert the matrix
+	 */
+	Matrix<M, N> inversed(void) const {
+        Matrix<M, N> res;
+        arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
+        return res;
+	}
+
+	/**
 	 * setup the identity matrix
 	 */
 	void identity(void) {
@@ -224,6 +242,8 @@ public:
 template <unsigned int M, unsigned int N>
 class Matrix : public MatrixBase<M, N> {
 public:
+	using MatrixBase<M, N>::operator *;
+
 	/**
 	 * set to value
 	 */
@@ -235,18 +255,18 @@ public:
 	/**
 	 * multiplication by a vector
 	 */
-	/*
 	Vector<N> operator *(const Vector<N> &v) const {
 	    Vector<M> res;
 	    arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
 	    return res;
 	}
-	*/
 };
 
 template <>
 class Matrix<3, 3> : public MatrixBase<3, 3> {
 public:
+	using MatrixBase<3, 3>::operator *;
+
 	/**
 	 * set to value
 	 */
@@ -258,15 +278,11 @@ public:
 	/**
 	 * multiplication by a vector
 	 */
-	/*
 	Vector<3> operator *(const Vector<3> &v) const {
-	    Vector<3> res;
-	    res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2);
-	    res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2);
-	    res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2);
-	    return res;
+	    return Vector<3>(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]);
 	}
-	*/
 };
 
 }
diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp
index 3236cd3d1..c7eb67ba7 100644
--- a/src/lib/mathlib/math/Matrix3.hpp
+++ b/src/lib/mathlib/math/Matrix3.hpp
@@ -74,14 +74,15 @@ public:
 	 * setting ctor
 	 */
 	Matrix3<T>(const T d[3][3]) {
-		memcpy(data, d, sizeof(data));
 		arm_mat = {3, 3, &data[0][0]};
+		memcpy(data, d, sizeof(data));
 	}
 
 	/**
 	 * setting ctor
 	 */
 	Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) {
+		arm_mat = {3, 3, &data[0][0]};
 		data[0][0] = ax;
 		data[0][1] = ay;
 		data[0][2] = az;
@@ -91,7 +92,6 @@ public:
 		data[2][0] = cx;
 		data[2][1] = cy;
 		data[2][2] = cz;
-		arm_mat = {3, 3, &data[0][0]};
 	}
 
 	/**
@@ -125,6 +125,14 @@ public:
 	}
 
 	/**
+	 * set to value
+	 */
+	const Matrix3<T> &operator =(const Matrix3<T> &m) {
+		memcpy(data, m.data, sizeof(data));
+		return *this;
+	}
+
+	/**
 	 * test for equality
 	 */
 	bool operator ==(const Matrix3<T> &m) {
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index adb2293e1..cd00058b5 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -49,7 +49,7 @@ namespace math
 {
 
 template <unsigned int N>
-class Vector {
+class VectorBase {
 public:
 	float data[N];
 	arm_matrix_instance_f32 arm_col;
@@ -57,15 +57,22 @@ public:
 	/**
 	 * trivial ctor
 	 */
-	Vector<N>() {
+	VectorBase<N>() {
 		arm_col = {N, 1, &data[0]};
 	}
 
 	/**
 	 * setting ctor
 	 */
-	Vector<N>(const float *d) {
-		memcpy(data, d, sizeof(data));
+//	VectorBase<N>(const float *d) {
+	//	memcpy(data, d, sizeof(data));
+		//arm_col = {N, 1, &data[0]};
+	//}
+
+	/**
+	 * setting ctor
+	 */
+	VectorBase<N>(const float d[]) : data(d) {
 		arm_col = {N, 1, &data[0]};
 	}
 
@@ -86,7 +93,7 @@ public:
 	/**
 	 * test for equality
 	 */
-	bool operator ==(const Vector<N> &v) {
+	bool operator ==(const VectorBase<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			if (data[i] != v(i))
  				return false;
@@ -96,7 +103,7 @@ public:
 	/**
 	 * test for inequality
 	 */
-	bool operator !=(const Vector<N> &v) {
+	bool operator !=(const VectorBase<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			if (data[i] != v(i))
  				return true;
@@ -106,7 +113,7 @@ public:
 	/**
 	 * set to value
 	 */
-	const Vector<N> &operator =(const Vector<N> &v) {
+	const VectorBase<N> &operator =(const VectorBase<N> &v) {
 		memcpy(data, v.data, sizeof(data));
 		return *this;
 	}
@@ -114,8 +121,8 @@ public:
 	/**
 	 * negation
 	 */
-	const Vector<N> operator -(void) const {
-		Vector<N> res;
+	const VectorBase<N> operator -(void) const {
+		VectorBase<N> res;
  		for (unsigned int i = 0; i < N; i++)
  			res[i] = -data[i];
  		return res;
@@ -124,8 +131,8 @@ public:
 	/**
 	 * addition
 	 */
-	const Vector<N> operator +(const Vector<N> &v) const {
-		Vector<N> res;
+	const VectorBase<N> operator +(const VectorBase<N> &v) const {
+		VectorBase<N> res;
  		for (unsigned int i = 0; i < N; i++)
  			res[i] = data[i] + v(i);
  		return res;
@@ -134,8 +141,8 @@ public:
 	/**
 	 * subtraction
 	 */
-	const Vector<N> operator -(const Vector<N> &v) const {
-		Vector<N> res;
+	const VectorBase<N> operator -(const VectorBase<N> &v) const {
+		VectorBase<N> res;
  		for (unsigned int i = 0; i < N; i++)
  			res[i] = data[i] - v(i);
  		return res;
@@ -144,23 +151,23 @@ public:
 	/**
 	 * uniform scaling
 	 */
-	const Vector<N> operator *(const float num) const {
-		Vector<N> temp(*this);
+	const VectorBase<N> operator *(const float num) const {
+		VectorBase<N> temp(*this);
 		return temp *= num;
 	}
 
 	/**
 	 * uniform scaling
 	 */
-	const Vector<N> operator /(const float num) const {
-		Vector<N> temp(*this);
+	const VectorBase<N> operator /(const float num) const {
+		VectorBase<N> temp(*this);
 		return temp /= num;
 	}
 
 	/**
 	 * addition
 	 */
-	const Vector<N> &operator +=(const Vector<N> &v) {
+	const VectorBase<N> &operator +=(const VectorBase<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] += v(i);
 		return *this;
@@ -169,7 +176,7 @@ public:
 	/**
 	 * subtraction
 	 */
-	const Vector<N> &operator -=(const Vector<N> &v) {
+	const VectorBase<N> &operator -=(const VectorBase<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] -= v(i);
 		return *this;
@@ -178,7 +185,7 @@ public:
 	/**
 	 * uniform scaling
 	 */
-	const Vector<N> &operator *=(const float num) {
+	const VectorBase<N> &operator *=(const float num) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] *= num;
 		return *this;
@@ -187,7 +194,7 @@ public:
 	/**
 	 * uniform scaling
 	 */
-	const Vector<N> &operator /=(const float num) {
+	const VectorBase<N> &operator /=(const float num) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] /= num;
 		return *this;
@@ -196,7 +203,7 @@ public:
 	/**
 	 * dot product
 	 */
-	float operator *(const Vector<N> &v) const {
+	float operator *(const VectorBase<N> &v) const {
 		float res;
  		for (unsigned int i = 0; i < N; i++)
  			res += data[i] * v(i);
@@ -227,11 +234,48 @@ public:
 	/**
 	 * returns the normalized version of this vector
 	 */
-	Vector<N> normalized() const {
+	VectorBase<N> normalized() const {
 		return *this / length();
 	}
 };
 
+template <unsigned int N>
+class Vector : public VectorBase<N> {
+public:
+	/**
+	 * set to value
+	 */
+	const Vector<N> &operator =(const Vector<N> &v) {
+		memcpy(this->data, v.data, sizeof(this->data));
+		return *this;
+	}
+};
+
+template <>
+class Vector<3> : public VectorBase<3> {
+public:
+	Vector<3>() {
+		arm_col = {3, 1, &this->data[0]};
+	}
+
+	Vector<3>(const float x, const float y, const float z) {
+		data[0] = x;
+		data[1] = y;
+		data[2] = z;
+		arm_col = {3, 1, &this->data[0]};
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<3> &operator =(const Vector<3> &v) {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+		data[2] = v.data[2];
+		return *this;
+	}
+};
+
 }
 
 #endif // VECTOR_HPP
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 13e6dad51..305ebbefa 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -58,15 +58,31 @@ int test_mathlib(int argc, char *argv[])
 {
 	warnx("testing mathlib");
 
-	Matrix3f m;
-	m.identity();
-	Matrix3f m1;
-	Matrix<3,3> mq;
-	mq.identity();
-	Matrix<3,3> mq1;
-	m1(0, 0) = 5.0;
-	Vector3f v = Vector3f(1.0f, 2.0f, 3.0f);
-	Vector3f v1;
+	Matrix<3,3> m3;
+	m3.identity();
+	Matrix<4,4> m4;
+	m4.identity();
+	Vector<3> v3;
+	v3(0) = 1.0f;
+	v3(1) = 2.0f;
+	v3(2) = 3.0f;
+	Vector<4> v4;
+	v4(0) = 1.0f;
+	v4(1) = 2.0f;
+	v4(2) = 3.0f;
+	v4(3) = 4.0f;
+	Vector<3> vres3;
+	Matrix<3,3> mres3;
+	Matrix<4,4> mres4;
+
+	Matrix3f m3old;
+	m3old.identity();
+	Vector3f v3old;
+	v3old.x = 1.0f;
+	v3old.y = 2.0f;
+	v3old.z = 3.0f;
+	Vector3f vres3old;
+	Matrix3f mres3old;
 
 	unsigned int n = 60000;
 
@@ -74,41 +90,51 @@ int test_mathlib(int argc, char *argv[])
 
 	t0 = hrt_absolute_time();
 	for (unsigned int j = 0; j < n; j++) {
-		v1 = m * v;
+		vres3 = m3 * v3;
 	}
 	t1 = hrt_absolute_time();
-	warnx("Matrix * Vector: %s %.6fus", formatResult(v1 == v), (double)(t1 - t0) / n);
+	warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n);
 
 	t0 = hrt_absolute_time();
 	for (unsigned int j = 0; j < n; j++) {
-		mq1 = mq * mq;
+		vres3old = m3old * v3old;
 	}
 	t1 = hrt_absolute_time();
-	warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n);
-	mq1.dump();
+	warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n);
 
 	t0 = hrt_absolute_time();
 	for (unsigned int j = 0; j < n; j++) {
-		m1 = m.transposed();
+		mres3 = m3 * m3;
 	}
 	t1 = hrt_absolute_time();
-	warnx("Matrix Transpose: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n);
+	warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n);
 
 	t0 = hrt_absolute_time();
 	for (unsigned int j = 0; j < n; j++) {
-		m1 = m.inversed();
+		mres3old = m3old * m3old;
 	}
 	t1 = hrt_absolute_time();
-	warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n);
-
-	Matrix<4,4> mn;
-	mn(0, 0) = 2.0f;
-	mn(1, 0) = 3.0f;
-	for (int i = 0; i < mn.getRows(); i++) {
-		for (int j = 0; j < mn.getCols(); j++) {
-			printf("%.3f ", mn(i, j));
-		}
-		printf("\n");
+	warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n);
+
+	t0 = hrt_absolute_time();
+	for (unsigned int j = 0; j < n; j++) {
+		mres4 = m4 * m4;
 	}
+	t1 = hrt_absolute_time();
+	warnx("Matrix4 * Matrix4: %s %.6fus", formatResult(mres4 == m4), (double)(t1 - t0) / n);
+
+	t0 = hrt_absolute_time();
+	for (unsigned int j = 0; j < n; j++) {
+		mres3 = m3.transposed();
+	}
+	t1 = hrt_absolute_time();
+	warnx("Matrix3 Transpose: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n);
+
+	t0 = hrt_absolute_time();
+	for (unsigned int j = 0; j < n; j++) {
+		mres3 = m3.inversed();
+	}
+	t1 = hrt_absolute_time();
+	warnx("Matrix3 Invert: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n);
 	return 0;
 }