mathlib: fixes and improvements, WIP

3 files changed, 104 insertions, 36 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