mathlib: Matrix and Quaternion cleanup and bugfixes. Copyright updated.

5 files changed, 114 insertions, 75 deletions

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 9896a16d0..584e5e81b 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -1,8 +1,9 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Will Perone <will.perone@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +37,7 @@
 /**
  * @file Matrix.hpp
  *
- * Generic Matrix
+ * Matrix class
  */
 
 #ifndef MATRIX_HPP
@@ -51,8 +52,6 @@ namespace math
 template <unsigned int M, unsigned int N>
 class __EXPORT Matrix;
 
-class __EXPORT Quaternion;
-
 // MxN matrix with float elements
 template <unsigned int M, unsigned int N>
 class __EXPORT MatrixBase {
@@ -75,6 +74,14 @@ public:
 		arm_mat = {M, N, &data[0][0]};
 	}
 
+	/**
+	 * copyt ctor
+	 */
+	MatrixBase(const MatrixBase<M, N> &m) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, m.data, sizeof(data));
+	}
+
 	MatrixBase(const float *d) {
 		arm_mat = {M, N, &data[0][0]};
 		memcpy(data, d, sizeof(data));
@@ -83,29 +90,35 @@ public:
 	/**
 	 * access by index
 	 */
-	inline float &operator ()(unsigned int row, unsigned int col) {
+	float &operator ()(const unsigned int row, const unsigned int col) {
 		return data[row][col];
 	}
 
 	/**
 	 * access by index
 	 */
-	inline const float &operator ()(unsigned int row, unsigned int col) const {
+	float operator ()(const unsigned int row, const unsigned int col) const {
 		return data[row][col];
 	}
 
-	unsigned int get_rows() {
+	/**
+	 * get rows number
+	 */
+	unsigned int get_rows() const {
 		return M;
 	}
 
-	unsigned int get_cols() {
+	/**
+	 * get columns number
+	 */
+	unsigned int get_cols() const {
 		return N;
 	}
 
 	/**
 	 * test for equality
 	 */
-	bool operator ==(const Matrix<M, N> &m) {
+	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])
@@ -116,7 +129,7 @@ public:
 	/**
 	 * test for inequality
 	 */
-	bool operator !=(const Matrix<M, N> &m) {
+	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])
@@ -209,7 +222,7 @@ public:
  		for (unsigned int i = 0; i < M; i++)
  			for (unsigned int j = 0; j < N; j++)
  				data[i][j] /= num;
-		return *this;
+		return *static_cast<Matrix<M, N>*>(this);
 	}
 
 	/**
@@ -272,11 +285,11 @@ class __EXPORT Matrix : public MatrixBase<M, N> {
 public:
 	using MatrixBase<M, N>::operator *;
 
-	Matrix() : MatrixBase<M, N>() {
-	}
+	Matrix() : MatrixBase<M, N>() {}
 
-	Matrix(const float *d) : MatrixBase<M, N>(d) {
-	}
+	Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
+
+	Matrix(const float *d) : MatrixBase<M, N>(d) {}
 
 	/**
 	 * set to value
@@ -301,11 +314,11 @@ class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> {
 public:
 	using MatrixBase<3, 3>::operator *;
 
-	Matrix() : MatrixBase<3, 3>() {
-	}
+	Matrix() : MatrixBase<3, 3>() {}
 
-	Matrix(const float *d) : MatrixBase<3, 3>(d) {
-	}
+	Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
+
+	Matrix(const float *d) : MatrixBase<3, 3>(d) {}
 
 	/**
 	 * set to value
@@ -348,6 +361,9 @@ public:
 		data[2][2] = cr * cp;
 	}
 
+	/**
+	 * get euler angles from rotation matrix
+	 */
 	Vector<3> to_euler(void) const {
 		Vector<3> euler;
 		euler.data[1] = asinf(-data[2][0]);
@@ -364,6 +380,7 @@ public:
 			euler.data[0] = atan2f(data[2][1], data[2][2]);
 			euler.data[2] = atan2f(data[1][0], data[0][0]);
 		}
+		return euler;
 	}
 };
 
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index c19dbd29c..54d4e72ab 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -1,8 +1,9 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Will Perone <will.perone@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +37,7 @@
 /**
  * @file Quaternion.hpp
  *
- * Quaternion
+ * Quaternion class
  */
 
 #ifndef QUATERNION_HPP
@@ -50,31 +51,32 @@
 namespace math
 {
 
-template <unsigned int N, unsigned int M>
-class Matrix;
-
-class Quaternion : public Vector<4> {
+class __EXPORT Quaternion : public Vector<4> {
 public:
 	/**
 	 * trivial ctor
 	 */
-	Quaternion() : Vector() {
-	}
+	Quaternion() : Vector<4>() {}
 
 	/**
-	 * setting ctor
+	 * copy ctor
 	 */
-	Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) {
-	}
+	Quaternion(const Quaternion &q) : Vector<4>(q) {}
 
-	Quaternion(const Vector<4> &v) : Vector(v) {
-	}
+	/**
+	 * casting from vector
+	 */
+	Quaternion(const Vector<4> &v) : Vector<4>(v) {}
 
-	Quaternion(const Quaternion &q) : Vector(q) {
-	}
+	/**
+	 * setting ctor
+	 */
+	Quaternion(const float d[4]) : Vector<4>(d) {}
 
-	Quaternion(const float v[4]) : Vector(v) {
-	}
+	/**
+	 * setting ctor
+	 */
+	Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {}
 
 	using Vector<4>::operator *;
 
@@ -138,8 +140,10 @@ public:
 		R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
 		R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
 		R.data[2][2] = aSq - bSq - cSq + dSq;
+		return R;
 	}
 };
+
 }
 
 #endif // QUATERNION_HPP
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index d579ecf73..6bfcc96b6 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -68,6 +68,7 @@ public:
 
 	/**
 	 * trivial ctor
+	 * note that this ctor will not initialize elements
 	 */
 	VectorBase() {
 		arm_col = {N, 1, &data[0]};
@@ -104,7 +105,7 @@ public:
 	}
 
 	/**
-	 * get rows number
+	 * get vector size
 	 */
 	unsigned int get_size() const {
 		return N;
@@ -113,7 +114,7 @@ public:
 	/**
 	 * test for equality
 	 */
-	bool operator ==(const Vector<N> &v) {
+	bool operator ==(const Vector<N> &v) const {
  		for (unsigned int i = 0; i < N; i++)
  			if (data[i] != v.data[i])
  				return false;
@@ -123,7 +124,7 @@ public:
 	/**
 	 * test for inequality
 	 */
-	bool operator !=(const Vector<N> &v) {
+	bool operator !=(const Vector<N> &v) const {
  		for (unsigned int i = 0; i < N; i++)
  			if (data[i] != v.data[i])
  				return true;
@@ -286,10 +287,9 @@ public:
 template <unsigned int N>
 class __EXPORT Vector : public VectorBase<N> {
 public:
-	//using VectorBase<N>::operator *;
 	Vector() : VectorBase<N>() {}
 
-	Vector(const Vector &v) : VectorBase<N>(v) {}
+	Vector(const Vector<N> &v) : VectorBase<N>(v) {}
 
 	Vector(const float d[N]) : VectorBase<N>(d) {}
 
@@ -307,8 +307,8 @@ class __EXPORT Vector<2> : public VectorBase<2> {
 public:
 	Vector() : VectorBase<2>() {}
 
-	/* simple copy is 1.6 times faster than memcpy */
-	Vector(const Vector &v) : VectorBase<2>() {
+	// simple copy is 1.6 times faster than memcpy
+	Vector(const Vector<2> &v) : VectorBase<2>() {
 		data[0] = v.data[0];
 		data[1] = v.data[1];
 	}
@@ -342,8 +342,8 @@ class __EXPORT Vector<3> : public VectorBase<3> {
 public:
 	Vector() : VectorBase<3>() {}
 
-	/* simple copy is 1.6 times faster than memcpy */
-	Vector(const Vector &v) : VectorBase<3>() {
+	// simple copy is 1.6 times faster than memcpy
+	Vector(const Vector<3> &v) : VectorBase<3>() {
 		for (unsigned int i = 0; i < 3; i++)
 			data[i] = v.data[i];
 	}
@@ -382,7 +382,7 @@ class __EXPORT Vector<4> : public VectorBase<4> {
 public:
 	Vector() : VectorBase() {}
 
-	Vector(const Vector &v) : VectorBase<4>() {
+	Vector(const Vector<4> &v) : VectorBase<4>() {
 		for (unsigned int i = 0; i < 4; i++)
 			data[i] = v.data[i];
 	}
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 70ba628f3..3d20d4d2d 100644
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 30,
-					 8096,
+					 8192,
 					 kalman_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index e654e0f81..693a208ba 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -60,25 +60,15 @@ int test_mathlib(int argc, char *argv[])
 {
 	warnx("testing mathlib");
 
-	Vector<2> v2(1.0f, 2.0f);
-	Vector<3> v3(1.0f, 2.0f, 3.0f);
-	Vector<4> v4(1.0f, 2.0f, 3.0f, 4.0f);
-	Vector<10> v10;
-	v10.zero();
-
-	float data2[2] = {1.0f, 2.0f};
-	float data3[3] = {1.0f, 2.0f, 3.0f};
-	float data4[4] = {1.0f, 2.0f, 3.0f, 4.0f};
-	float data10[10];
-
 	{
 		Vector<2> v;
 		Vector<2> v1(1.0f, 2.0f);
 		Vector<2> v2(1.0f, -1.0f);
-		TEST_OP("Constructor Vector<2>()", Vector<2> v);
-		TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v(v2));
-		TEST_OP("Constructor Vector<2>(float[])", Vector<2> v(data2));
-		TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v(1.0f, 2.0f));
+		float data[2] = {1.0f, 2.0f};
+		TEST_OP("Constructor Vector<2>()", Vector<2> v3);
+		TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1));
+		TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data));
+		TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f));
 		TEST_OP("Vector<2> = Vector<2>", v = v1);
 		TEST_OP("Vector<2> + Vector<2>", v + v1);
 		TEST_OP("Vector<2> - Vector<2>", v - v1);
@@ -92,10 +82,11 @@ int test_mathlib(int argc, char *argv[])
 		Vector<3> v;
 		Vector<3> v1(1.0f, 2.0f, 0.0f);
 		Vector<3> v2(1.0f, -1.0f, 2.0f);
-		TEST_OP("Constructor Vector<3>()", Vector<3> v);
-		TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v(v3));
-		TEST_OP("Constructor Vector<3>(float[])", Vector<3> v(data3));
-		TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v(1.0f, 2.0f, 3.0f));
+		float data[3] = {1.0f, 2.0f, 3.0f};
+		TEST_OP("Constructor Vector<3>()", Vector<3> v3);
+		TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1));
+		TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data));
+		TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f));
 		TEST_OP("Vector<3> = Vector<3>", v = v1);
 		TEST_OP("Vector<3> + Vector<3>", v + v1);
 		TEST_OP("Vector<3> - Vector<3>", v - v1);
@@ -119,10 +110,11 @@ int test_mathlib(int argc, char *argv[])
 		Vector<4> v;
 		Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
 		Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f);
-		TEST_OP("Constructor Vector<4>()", Vector<4> v);
-		TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v(v4));
-		TEST_OP("Constructor Vector<4>(float[])", Vector<4> v(data4));
-		TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v(1.0f, 2.0f, 3.0f, 4.0f));
+		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
+		TEST_OP("Constructor Vector<4>()", Vector<4> v3);
+		TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1));
+		TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data));
+		TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f));
 		TEST_OP("Vector<4> = Vector<4>", v = v1);
 		TEST_OP("Vector<4> + Vector<4>", v + v1);
 		TEST_OP("Vector<4> - Vector<4>", v - v1);
@@ -132,9 +124,35 @@ int test_mathlib(int argc, char *argv[])
 	}
 
 	{
-		TEST_OP("Constructor Vector<10>()", Vector<10> v);
-		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v(v10));
-		TEST_OP("Constructor Vector<10>(float[])", Vector<10> v(data10));
+		Vector<10> v1;
+		v1.zero();
+		float data[10];
+		TEST_OP("Constructor Vector<10>()", Vector<10> v3);
+		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
+		TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
+	}
+
+	{
+		Matrix<3, 3> m1;
+		m1.identity();
+		Matrix<3, 3> m2;
+		m2.identity();
+		Vector<3> v1(1.0f, 2.0f, 0.0f);
+		TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1);
+		TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
+		TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
+	}
+
+	{
+		Matrix<10, 10> m1;
+		m1.identity();
+		Matrix<10, 10> m2;
+		m2.identity();
+		Vector<10> v1;
+		v1.zero();
+		TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
+		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
+		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
 	}
 
 	return 0;