diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-18 23:01:02 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-18 23:01:02 +0400 |
commit | e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940 (patch) | |
tree | 1f90de01a4f5b1be1eb827981330400c43242013 /src | |
parent | a83e3cd22276109301678c204e83050483200d6b (diff) | |
download | px4-firmware-e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940.tar.gz px4-firmware-e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940.tar.bz2 px4-firmware-e3a5a384d7b3678d1cbef63dc28fbe9a8f1de940.zip |
mathlib: fixes and improvements, WIP
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 38 | ||||
-rw-r--r-- | src/lib/mathlib/math/Matrix3.hpp | 12 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 90 | ||||
-rw-r--r-- | src/systemcmds/tests/test_mathlib.cpp | 80 |
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; } |