diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 584e5e81bf..2a5eac79f0 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -54,7 +54,8 @@ class __EXPORT Matrix; // MxN matrix with float elements template -class __EXPORT MatrixBase { +class __EXPORT MatrixBase +{ public: /** * matrix data[row][col] @@ -90,14 +91,14 @@ public: /** * access by index */ - float &operator ()(const unsigned int row, const unsigned int col) { + float &operator()(const unsigned int row, const unsigned int col) { return data[row][col]; } /** * access by index */ - float operator ()(const unsigned int row, const unsigned int col) const { + float operator()(const unsigned int row, const unsigned int col) const { return data[row][col]; } @@ -119,10 +120,11 @@ public: * test for equality */ bool operator ==(const Matrix &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]) - return false; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; + return true; } @@ -130,10 +132,11 @@ public: * test for inequality */ bool operator !=(const Matrix &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]) - return true; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; + return false; } @@ -150,9 +153,11 @@ public: */ Matrix operator -(void) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = -data[i][j]; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; + return res; } @@ -161,16 +166,19 @@ public: */ Matrix operator +(const Matrix &m) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - res.data[i][j] = data[i][j] + m.data[i][j]; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; + return res; } Matrix &operator +=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] += m.data[i][j]; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + return *static_cast*>(this); } @@ -179,16 +187,19 @@ public: */ Matrix operator -(const Matrix &m) const { Matrix res; - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] - m.data[i][j]; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; + return res; } Matrix &operator -=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) - data[i][j] -= m.data[i][j]; + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + return *static_cast*>(this); } @@ -197,31 +208,37 @@ public: */ Matrix operator *(const float num) const { Matrix res; - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res.data[i][j] = data[i][j] * num; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; + return res; } Matrix &operator *=(const float num) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] *= num; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + return *static_cast*>(this); } Matrix operator /(const float num) const { Matrix res; - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] / num; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + return res; } Matrix &operator /=(const float num) { - for (unsigned int i = 0; i < M; i++) - for (unsigned int j = 0; j < N; j++) - data[i][j] /= num; + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + return *static_cast*>(this); } @@ -239,18 +256,18 @@ public: * transpose the matrix */ Matrix transposed(void) const { - Matrix res; - arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); - return res; + Matrix res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; } /** * invert the matrix */ Matrix inversed(void) const { - Matrix res; - arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); - return res; + Matrix res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; } /** @@ -266,22 +283,26 @@ public: void identity(void) { memset(data, 0, sizeof(data)); unsigned int n = (M < N) ? M : N; + for (unsigned int i = 0; i < n; i++) data[i][i] = 1; } void print(void) { - for (unsigned int i = 0; i < M; i++) { + for (unsigned int i = 0; i < M; i++) { printf("[ "); - for (unsigned int j = 0; j < N; j++) + + for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); + printf(" ]\n"); } } }; template -class __EXPORT Matrix : public MatrixBase { +class __EXPORT Matrix : public MatrixBase +{ public: using MatrixBase::operator *; @@ -303,14 +324,15 @@ public: * multiplication by a vector */ Vector operator *(const Vector &v) const { - Vector res; - arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); - return res; + Vector res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; } }; template <> -class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> { +class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> +{ public: using MatrixBase<3, 3>::operator *; @@ -332,10 +354,10 @@ public: * multiplication by a vector */ Vector<3> operator *(const Vector<3> &v) const { - Vector<3> res(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]); - return res; + Vector<3> res(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]); + return res; } /** @@ -380,6 +402,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 54d4e72ab9..77e1c1f1c6 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -51,7 +51,8 @@ namespace math { -class __EXPORT Quaternion : public Vector<4> { +class __EXPORT Quaternion : public Vector<4> +{ public: /** * trivial ctor @@ -85,25 +86,25 @@ public: */ 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]); + 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], - data[2], data[3], data[0], -data[1], - data[3], -data[2], data[1], data[0] - }; - Matrix<4,4> Q(dataQ); - Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); - return Q * v * 0.5f; + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4, 4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; } /** diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 6bfcc96b60..d75a05c980 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -54,7 +54,8 @@ template class __EXPORT Vector; template -class __EXPORT VectorBase { +class __EXPORT VectorBase +{ public: /** * vector data @@ -93,14 +94,14 @@ public: /** * access to elements by index */ - float &operator ()(const unsigned int i) { + float &operator()(const unsigned int i) { return data[i]; } /** * access to elements by index */ - float operator ()(const unsigned int i) const { + float operator()(const unsigned int i) const { return data[i]; } @@ -115,20 +116,22 @@ public: * test for equality */ bool operator ==(const Vector &v) const { - for (unsigned int i = 0; i < N; i++) - if (data[i] != v.data[i]) - return false; - return true; + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return false; + + return true; } /** * test for inequality */ bool operator !=(const Vector &v) const { - for (unsigned int i = 0; i < N; i++) - if (data[i] != v.data[i]) - return true; - return false; + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return true; + + return false; } /** @@ -144,9 +147,11 @@ public: */ const Vector operator -(void) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = -data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = -data[i]; + + return res; } /** @@ -154,9 +159,11 @@ public: */ const Vector operator +(const Vector &v) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] + v.data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] + v.data[i]; + + return res; } /** @@ -164,9 +171,11 @@ public: */ const Vector operator -(const Vector &v) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] - v.data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] - v.data[i]; + + return res; } /** @@ -174,8 +183,10 @@ public: */ const Vector operator *(const float num) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] * num; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + return res; } @@ -184,8 +195,10 @@ public: */ const Vector operator /(const float num) const { Vector res; - for (unsigned int i = 0; i < N; i++) - res.data[i] = data[i] / num; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + return res; } @@ -193,8 +206,9 @@ public: * addition */ const Vector &operator +=(const Vector &v) { - for (unsigned int i = 0; i < N; i++) - data[i] += v.data[i]; + for (unsigned int i = 0; i < N; i++) + data[i] += v.data[i]; + return *static_cast*>(this); } @@ -202,8 +216,9 @@ public: * subtraction */ const Vector &operator -=(const Vector &v) { - for (unsigned int i = 0; i < N; i++) - data[i] -= v.data[i]; + for (unsigned int i = 0; i < N; i++) + data[i] -= v.data[i]; + return *static_cast*>(this); } @@ -211,8 +226,9 @@ public: * uniform scaling */ const Vector &operator *=(const float num) { - for (unsigned int i = 0; i < N; i++) - data[i] *= num; + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + return *static_cast*>(this); } @@ -220,8 +236,9 @@ public: * uniform scaling */ const Vector &operator /=(const float num) { - for (unsigned int i = 0; i < N; i++) - data[i] /= num; + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + return *static_cast*>(this); } @@ -230,9 +247,11 @@ public: */ float operator *(const Vector &v) const { float res = 0.0f; - for (unsigned int i = 0; i < N; i++) - res += data[i] * v.data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * v.data[i]; + + return res; } /** @@ -240,9 +259,11 @@ public: */ float length_squared() const { float res = 0.0f; - for (unsigned int i = 0; i < N; i++) - res += data[i] * data[i]; - return res; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return res; } /** @@ -250,9 +271,11 @@ public: */ float length() const { float res = 0.0f; - for (unsigned int i = 0; i < N; i++) - res += data[i] * data[i]; - return sqrtf(res); + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return sqrtf(res); } /** @@ -278,14 +301,17 @@ public: void print(void) { printf("[ "); - for (unsigned int i = 0; i < N; i++) + + for (unsigned int i = 0; i < N; i++) printf("%.3f\t", data[i]); + printf("]\n"); } }; template -class __EXPORT Vector : public VectorBase { +class __EXPORT Vector : public VectorBase +{ public: Vector() : VectorBase() {} @@ -303,7 +329,8 @@ public: }; template <> -class __EXPORT Vector<2> : public VectorBase<2> { +class __EXPORT Vector<2> : public VectorBase<2> +{ public: Vector() : VectorBase<2>() {} @@ -333,12 +360,13 @@ public: } float operator %(const Vector<2> &v) const { - return data[0] * v.data[1] - data[1] * v.data[0]; + return data[0] * v.data[1] - data[1] * v.data[0]; } }; template <> -class __EXPORT Vector<3> : public VectorBase<3> { +class __EXPORT Vector<3> : public VectorBase<3> +{ public: Vector() : VectorBase<3>() {} @@ -365,20 +393,22 @@ public: const Vector<3> &operator =(const Vector<3> &v) { 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] - ); + 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 <> -class __EXPORT Vector<4> : public VectorBase<4> { +class __EXPORT Vector<4> : public VectorBase<4> +{ public: Vector() : VectorBase() {} @@ -405,6 +435,7 @@ public: const Vector<4> &operator =(const Vector<4> &v) { for (unsigned int i = 0; i < 4; i++) data[i] = v.data[i]; + return *this; } };