mathlib: Vector class major cleanup

3 files changed, 133 insertions, 137 deletions

diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 7ed8879a7..9896a16d0 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -94,11 +94,11 @@ public:
 		return data[row][col];
 	}
 
-	unsigned int getRows() {
+	unsigned int get_rows() {
 		return M;
 	}
 
-	unsigned int getCols() {
+	unsigned int get_cols() {
 		return N;
 	}
 
@@ -295,11 +295,7 @@ public:
 	    return res;
 	}
 };
-}
-
-#include "Quaternion.hpp"
 
-namespace math {
 template <>
 class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> {
 public:
@@ -330,25 +326,6 @@ public:
 	}
 
 	/**
-	 * create a rotation matrix from given quaternion
-	 */
-	void from_quaternion(const Quaternion &q) {
-		float aSq = q.data[0] * q.data[0];
-		float bSq = q.data[1] * q.data[1];
-		float cSq = q.data[2] * q.data[2];
-		float dSq = q.data[3] * q.data[3];
-		data[0][0] = aSq + bSq - cSq - dSq;
-		data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]);
-		data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]);
-		data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]);
-		data[1][1] = aSq - bSq + cSq - dSq;
-		data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]);
-		data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]);
-		data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]);
-		data[2][2] = aSq - bSq - cSq + dSq;
-	}
-
-	/**
 	 * create a rotation matrix from given euler angles
 	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
 	 */
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 3735fb3d3..c19dbd29c 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -58,7 +58,7 @@ public:
 	/**
 	 * trivial ctor
 	 */
-	Quaternion() {
+	Quaternion() : Vector() {
 	}
 
 	/**
@@ -70,10 +70,29 @@ public:
 	Quaternion(const Vector<4> &v) : Vector(v) {
 	}
 
-	Quaternion(const float *v) : Vector(v) {
+	Quaternion(const Quaternion &q) : Vector(q) {
 	}
 
-	Quaternion derivative(const Vector<3> &w) {
+	Quaternion(const float v[4]) : Vector(v) {
+	}
+
+	using Vector<4>::operator *;
+
+	/**
+	 * multiplication
+	 */
+	const Quaternion operator *(const Quaternion &q) const {
+		return Quaternion(
+				data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
+				data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
+				data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
+				data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
+	}
+
+	/**
+	 * derivative
+	 */
+	const Quaternion derivative(const Vector<3> &w) {
 	        float dataQ[] = {
 	                data[0], -data[1], -data[2], -data[3],
 	                data[1],  data[0], -data[3],  data[2],
@@ -85,6 +104,9 @@ public:
 	        return Q * v * 0.5f;
 	}
 
+	/**
+	 * set quaternion to rotation defined by euler angles
+	 */
 	void from_euler(float roll, float pitch, float yaw) {
 		double cosPhi_2 = cos(double(roll) / 2.0);
 		double sinPhi_2 = sin(double(roll) / 2.0);
@@ -97,6 +119,26 @@ public:
 		data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
 		data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
 	}
+
+	/**
+	 * create rotation matrix for the quaternion
+	 */
+	Matrix<3, 3> to_dcm(void) const {
+		Matrix<3, 3> R;
+		float aSq = data[0] * data[0];
+		float bSq = data[1] * data[1];
+		float cSq = data[2] * data[2];
+		float dSq = data[3] * data[3];
+		R.data[0][0] = aSq + bSq - cSq - dSq;
+		R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
+		R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
+		R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
+		R.data[1][1] = aSq - bSq + cSq - dSq;
+		R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
+		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;
+	}
 };
 }
 
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 744402e21..d579ecf73 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.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,13 +37,12 @@
 /**
  * @file Vector.hpp
  *
- * Generic Vector
+ * Vector class
  */
 
 #ifndef VECTOR_HPP
 #define VECTOR_HPP
 
-#include <visibility.h>
 #include <stdio.h>
 #include <math.h>
 #include "../CMSIS/Include/arm_math.h"
@@ -84,7 +84,7 @@ public:
 	/**
 	 * setting ctor
 	 */
-	VectorBase(const float *d) {
+	VectorBase(const float d[N]) {
 		arm_col = {N, 1, &data[0]};
 		memcpy(data, d, sizeof(data));
 	}
@@ -92,31 +92,30 @@ public:
 	/**
 	 * access to elements by index
 	 */
-	inline float &operator ()(unsigned int i) {
+	float &operator ()(const unsigned int i) {
 		return data[i];
 	}
 
 	/**
 	 * access to elements by index
 	 */
-	inline const float &operator ()(unsigned int i) const {
+	float operator ()(const unsigned int i) const {
 		return data[i];
 	}
 
-	unsigned int getRows() {
+	/**
+	 * get rows number
+	 */
+	unsigned int get_size() const {
 		return N;
 	}
 
-	unsigned int getCols() {
-		return 1;
-	}
-
 	/**
 	 * test for equality
 	 */
 	bool operator ==(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
- 			if (data[i] != v(i))
+ 			if (data[i] != v.data[i])
  				return false;
  		return true;
 	}
@@ -126,7 +125,7 @@ public:
 	 */
 	bool operator !=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
- 			if (data[i] != v(i))
+ 			if (data[i] != v.data[i])
  				return true;
  		return false;
 	}
@@ -155,7 +154,7 @@ public:
 	const Vector<N> operator +(const Vector<N> &v) const {
 		Vector<N> res;
  		for (unsigned int i = 0; i < N; i++)
- 			res.data[i] = data[i] + v(i);
+ 			res.data[i] = data[i] + v.data[i];
  		return res;
 	}
 
@@ -165,7 +164,7 @@ public:
 	const Vector<N> operator -(const Vector<N> &v) const {
 		Vector<N> res;
  		for (unsigned int i = 0; i < N; i++)
- 			res.data[i] = data[i] - v(i);
+ 			res.data[i] = data[i] - v.data[i];
  		return res;
 	}
 
@@ -173,16 +172,20 @@ public:
 	 * uniform scaling
 	 */
 	const Vector<N> operator *(const float num) const {
-		Vector<N> temp(*this);
-		return temp *= num;
+		Vector<N> res;
+ 		for (unsigned int i = 0; i < N; i++)
+ 			res.data[i] = data[i] * num;
+		return res;
 	}
 
 	/**
 	 * uniform scaling
 	 */
 	const Vector<N> operator /(const float num) const {
-		Vector<N> temp(*static_cast<const Vector<N>*>(this));
-		return temp /= num;
+		Vector<N> res;
+ 		for (unsigned int i = 0; i < N; i++)
+ 			res.data[i] = data[i] / num;
+		return res;
 	}
 
 	/**
@@ -190,7 +193,7 @@ public:
 	 */
 	const Vector<N> &operator +=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
- 			data[i] += v(i);
+ 			data[i] += v.data[i];
 		return *static_cast<const Vector<N>*>(this);
 	}
 
@@ -199,7 +202,7 @@ public:
 	 */
 	const Vector<N> &operator -=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
- 			data[i] -= v(i);
+ 			data[i] -= v.data[i];
 		return *static_cast<const Vector<N>*>(this);
 	}
 
@@ -227,7 +230,7 @@ public:
 	float operator *(const Vector<N> &v) const {
 		float res = 0.0f;
  		for (unsigned int i = 0; i < N; i++)
- 			res += data[i] * v(i);
+ 			res += data[i] * v.data[i];
  		return res;
 	}
 
@@ -235,14 +238,20 @@ public:
 	 * gets the length of this vector squared
 	 */
 	float length_squared() const {
-		return (*this * *this);
+		float res = 0.0f;
+ 		for (unsigned int i = 0; i < N; i++)
+ 			res += data[i] * data[i];
+ 		return res;
 	}
 
 	/**
 	 * gets the length of this vector
 	 */
 	float length() const {
-		return sqrtf(*this * *static_cast<const Vector<N>*>(this));
+		float res = 0.0f;
+ 		for (unsigned int i = 0; i < N; i++)
+ 			res += data[i] * data[i];
+ 		return sqrtf(res);
 	}
 
 	/**
@@ -277,25 +286,17 @@ public:
 template <unsigned int N>
 class __EXPORT Vector : public VectorBase<N> {
 public:
-	using VectorBase<N>::operator *;
+	//using VectorBase<N>::operator *;
+	Vector() : VectorBase<N>() {}
 
-	Vector() : VectorBase<N>() {
-	}
+	Vector(const Vector &v) : VectorBase<N>(v) {}
 
-	Vector(const float d[]) : VectorBase<N>(d) {
-	}
-
-	Vector(const Vector<N> &v) : VectorBase<N>(v) {
-	}
-
-	Vector(const VectorBase<N> &v) : VectorBase<N>(v) {
-	}
+	Vector(const float d[N]) : VectorBase<N>(d) {}
 
 	/**
 	 * set to value
 	 */
 	const Vector<N> &operator =(const Vector<N> &v) {
-		this->arm_col = {N, 1, &this->data[0]};
 		memcpy(this->data, v.data, sizeof(this->data));
 		return *this;
 	}
@@ -304,22 +305,22 @@ public:
 template <>
 class __EXPORT Vector<2> : public VectorBase<2> {
 public:
-	Vector() : VectorBase<2>() {
-	}
+	Vector() : VectorBase<2>() {}
 
-	Vector(const float x, const float y) : VectorBase() {
-		data[0] = x;
-		data[1] = y;
-	}
-
-	Vector(const Vector<2> &v) : VectorBase() {
+	/* simple copy is 1.6 times faster than memcpy */
+	Vector(const Vector &v) : VectorBase<2>() {
 		data[0] = v.data[0];
 		data[1] = v.data[1];
 	}
 
-	Vector(const VectorBase<2> &v) : VectorBase() {
-		data[0] = v.data[0];
-		data[1] = v.data[1];
+	Vector(const float d[2]) : VectorBase<2>() {
+		data[0] = d[0];
+		data[1] = d[1];
+	}
+
+	Vector(const float x, const float y) : VectorBase<2>() {
+		data[0] = x;
+		data[1] = y;
 	}
 
 	/**
@@ -331,55 +332,49 @@ public:
 		return *this;
 	}
 
-	float cross(const Vector<2> &b) const {
-	        return data[0]*b.data[1] - data[1]*b.data[0];
-	}
-
 	float operator %(const Vector<2> &v) const {
-	        return cross(v);
+        return data[0] * v.data[1] - data[1] * v.data[0];
 	}
-
 };
 
 template <>
 class __EXPORT Vector<3> : public VectorBase<3> {
 public:
-	Vector() {
-		arm_col = {3, 1, &this->data[0]};
-	}
+	Vector() : VectorBase<3>() {}
 
-	Vector(const float x, const float y, const float z) {
-		arm_col = {3, 1, &this->data[0]};
-		data[0] = x;
-		data[1] = y;
-		data[2] = z;
+	/* simple copy is 1.6 times faster than memcpy */
+	Vector(const Vector &v) : VectorBase<3>() {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = v.data[i];
 	}
 
-	Vector(const Vector<3> &v) : VectorBase<3>() {
-		data[0] = v.data[0];
-		data[1] = v.data[1];
-		data[2] = v.data[2];
+	Vector(const float d[3]) : VectorBase<3>() {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = d[i];
 	}
 
-	/**
-	 * setting ctor
-	 */
-	Vector(const float d[]) {
-		arm_col = {3, 1, &this->data[0]};
-		data[0] = d[0];
-		data[1] = d[1];
-		data[2] = d[2];
+	Vector(const float x, const float y, const float z) : VectorBase<3>() {
+		data[0] = x;
+		data[1] = y;
+		data[2] = z;
 	}
 
 	/**
 	 * 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];
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = v.data[i];
 		return *this;
 	}
+
+	Vector<3> operator %(const Vector<3> &v) const {
+		return Vector<3>(
+				data[1] * v.data[2] - data[2] * v.data[1],
+				data[2] * v.data[0] - data[0] * v.data[2],
+				data[0] * v.data[1] - data[1] * v.data[0]
+		);
+	}
 };
 
 template <>
@@ -387,49 +382,31 @@ class __EXPORT Vector<4> : public VectorBase<4> {
 public:
 	Vector() : VectorBase() {}
 
-	Vector(const float x, const float y, const float z, const float t) : VectorBase() {
-		data[0] = x;
-		data[1] = y;
-		data[2] = z;
-		data[3] = t;
+	Vector(const Vector &v) : VectorBase<4>() {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = v.data[i];
 	}
 
-	Vector(const Vector<4> &v) : VectorBase() {
-		data[0] = v.data[0];
-		data[1] = v.data[1];
-		data[2] = v.data[2];
-		data[3] = v.data[3];
+	Vector(const float d[4]) : VectorBase<4>() {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = d[i];
 	}
 
-	Vector(const VectorBase<4> &v) : VectorBase() {
-		data[0] = v.data[0];
-		data[1] = v.data[1];
-		data[2] = v.data[2];
-		data[3] = v.data[3];
+	Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() {
+		data[0] = x0;
+		data[1] = x1;
+		data[2] = x2;
+		data[3] = x3;
 	}
 
 	/**
-	 * setting ctor
-	 */
-	/*
-	Vector(const float d[]) {
-		arm_col = {3, 1, &this->data[0]};
-		data[0] = d[0];
-		data[1] = d[1];
-		data[2] = d[2];
-	}
-*/
-	/**
 	 * 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];
+	const Vector<4> &operator =(const Vector<4> &v) {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = v.data[i];
 		return *this;
 	}
-	*/
 };
 
 }