zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

mathlib: fixes and improvements, WIP

sbg
Anton Babushkin 11 years ago
parent
a83e3cd222
commit
e3a5a384d7
4 changed files with 157 additions and 63 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 38
      src/lib/mathlib/math/Matrix.hpp
  2. 12
      src/lib/mathlib/math/Matrix3.hpp
  3. 90
      src/lib/mathlib/math/Vector.hpp
  4. 78
      src/systemcmds/tests/test_mathlib.cpp

38
src/lib/mathlib/math/Matrix.hpp
Unescape View File

@ -34,9 +34,9 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file Matrix3.hpp * @file Matrix.hpp
* *
* 3x3 Matrix * Generic Matrix
*/ */
#ifndef MATRIX_HPP #ifndef MATRIX_HPP
@ -203,6 +203,24 @@ public:
return res; return res;
} }
/**
* 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 * setup the identity matrix
*/ */
@ -224,6 +242,8 @@ public:
template <unsigned int M, unsigned int N> template <unsigned int M, unsigned int N>
class Matrix : public MatrixBase<M, N> { class Matrix : public MatrixBase<M, N> {
public: public:
using MatrixBase<M, N>::operator *;
/** /**
* set to value * set to value
*/ */
@ -235,18 +255,18 @@ public:
/** /**
* multiplication by a vector * multiplication by a vector
*/ */
/*
Vector<N> operator *(const Vector<N> &v) const { Vector<N> operator *(const Vector<N> &v) const {
Vector<M> res; Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
return res; return res;
} }
*/
}; };
template <> template <>
class Matrix<3, 3> : public MatrixBase<3, 3> { class Matrix<3, 3> : public MatrixBase<3, 3> {
public: public:
using MatrixBase<3, 3>::operator *;
/** /**
* set to value * set to value
*/ */
@ -258,15 +278,11 @@ public:
/** /**
* multiplication by a vector * multiplication by a vector
*/ */
/*
Vector<3> operator *(const Vector<3> &v) const { Vector<3> operator *(const Vector<3> &v) const {
Vector<3> res; return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
res(0) = data[0][0] * v(0) + data[0][1] * v(1) + data[0][2] * v(2); data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
res(1) = data[1][0] * v(0) + data[1][1] * v(1) + data[1][2] * v(2); data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
res(2) = data[2][0] * v(0) + data[2][1] * v(1) + data[2][2] * v(2);
return res;
} }
*/
}; };
} }

12
src/lib/mathlib/math/Matrix3.hpp
Unescape View File

@ -74,14 +74,15 @@ public:
* setting ctor * setting ctor
*/ */
Matrix3<T>(const T d[3][3]) { Matrix3<T>(const T d[3][3]) {
memcpy(data, d, sizeof(data));
arm_mat = {3, 3, &data[0][0]}; arm_mat = {3, 3, &data[0][0]};
memcpy(data, d, sizeof(data));
} }
/** /**
* setting ctor * 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) { 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][0] = ax;
data[0][1] = ay; data[0][1] = ay;
data[0][2] = az; data[0][2] = az;
@ -91,7 +92,6 @@ public:
data[2][0] = cx; data[2][0] = cx;
data[2][1] = cy; data[2][1] = cy;
data[2][2] = cz; data[2][2] = cz;
arm_mat = {3, 3, &data[0][0]};
} }
/** /**
@ -124,6 +124,14 @@ public:
return data[row][col]; return data[row][col];
} }
/**
* set to value
*/
const Matrix3<T> &operator =(const Matrix3<T> &m) {
memcpy(data, m.data, sizeof(data));
return *this;
}
/** /**
* test for equality * test for equality
*/ */

90
src/lib/mathlib/math/Vector.hpp
Unescape View File

@ -49,7 +49,7 @@ namespace math
{ {
template <unsigned int N> template <unsigned int N>
class Vector { class VectorBase {
public: public:
float data[N]; float data[N];
arm_matrix_instance_f32 arm_col; arm_matrix_instance_f32 arm_col;
@ -57,15 +57,22 @@ public:
/** /**
* trivial ctor * trivial ctor
*/ */
Vector<N>() { VectorBase<N>() {
arm_col = {N, 1, &data[0]}; arm_col = {N, 1, &data[0]};
} }
/** /**
* setting ctor * setting ctor
*/ */
Vector<N>(const float *d) { // VectorBase<N>(const float *d) {
memcpy(data, d, sizeof(data)); // 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]}; arm_col = {N, 1, &data[0]};
} }
@ -86,7 +93,7 @@ public:
/** /**
* test for equality * test for equality
*/ */
bool operator ==(const Vector<N> &v) { bool operator ==(const VectorBase<N> &v) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
if (data[i] != v(i)) if (data[i] != v(i))
return false; return false;
@ -96,7 +103,7 @@ public:
/** /**
* test for inequality * test for inequality
*/ */
bool operator !=(const Vector<N> &v) { bool operator !=(const VectorBase<N> &v) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
if (data[i] != v(i)) if (data[i] != v(i))
return true; return true;
@ -106,7 +113,7 @@ public:
/** /**
* set to value * 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)); memcpy(data, v.data, sizeof(data));
return *this; return *this;
} }
@ -114,8 +121,8 @@ public:
/** /**
* negation * negation
*/ */
const Vector<N> operator -(void) const { const VectorBase<N> operator -(void) const {
Vector<N> res; VectorBase<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
res[i] = -data[i]; res[i] = -data[i];
return res; return res;
@ -124,8 +131,8 @@ public:
/** /**
* addition * addition
*/ */
const Vector<N> operator +(const Vector<N> &v) const { const VectorBase<N> operator +(const VectorBase<N> &v) const {
Vector<N> res; VectorBase<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
res[i] = data[i] + v(i); res[i] = data[i] + v(i);
return res; return res;
@ -134,8 +141,8 @@ public:
/** /**
* subtraction * subtraction
*/ */
const Vector<N> operator -(const Vector<N> &v) const { const VectorBase<N> operator -(const VectorBase<N> &v) const {
Vector<N> res; VectorBase<N> res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
res[i] = data[i] - v(i); res[i] = data[i] - v(i);
return res; return res;
@ -144,23 +151,23 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> operator *(const float num) const { const VectorBase<N> operator *(const float num) const {
Vector<N> temp(*this); VectorBase<N> temp(*this);
return temp *= num; return temp *= num;
} }
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> operator /(const float num) const { const VectorBase<N> operator /(const float num) const {
Vector<N> temp(*this); VectorBase<N> temp(*this);
return temp /= num; return temp /= num;
} }
/** /**
* addition * 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++) for (unsigned int i = 0; i < N; i++)
data[i] += v(i); data[i] += v(i);
return *this; return *this;
@ -169,7 +176,7 @@ public:
/** /**
* subtraction * 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++) for (unsigned int i = 0; i < N; i++)
data[i] -= v(i); data[i] -= v(i);
return *this; return *this;
@ -178,7 +185,7 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> &operator *=(const float num) { const VectorBase<N> &operator *=(const float num) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
data[i] *= num; data[i] *= num;
return *this; return *this;
@ -187,7 +194,7 @@ public:
/** /**
* uniform scaling * uniform scaling
*/ */
const Vector<N> &operator /=(const float num) { const VectorBase<N> &operator /=(const float num) {
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
data[i] /= num; data[i] /= num;
return *this; return *this;
@ -196,7 +203,7 @@ public:
/** /**
* dot product * dot product
*/ */
float operator *(const Vector<N> &v) const { float operator *(const VectorBase<N> &v) const {
float res; float res;
for (unsigned int i = 0; i < N; i++) for (unsigned int i = 0; i < N; i++)
res += data[i] * v(i); res += data[i] * v(i);
@ -227,11 +234,48 @@ public:
/** /**
* returns the normalized version of this vector * returns the normalized version of this vector
*/ */
Vector<N> normalized() const { VectorBase<N> normalized() const {
return *this / length(); 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 #endif // VECTOR_HPP

78
src/systemcmds/tests/test_mathlib.cpp
Unescape View File

@ -58,15 +58,31 @@ int test_mathlib(int argc, char *argv[])
{ {
warnx("testing mathlib"); warnx("testing mathlib");
Matrix3f m; Matrix<3,3> m3;
m.identity(); m3.identity();
Matrix3f m1; Matrix<4,4> m4;
Matrix<3,3> mq; m4.identity();
mq.identity(); Vector<3> v3;
Matrix<3,3> mq1; v3(0) = 1.0f;
m1(0, 0) = 5.0; v3(1) = 2.0f;
Vector3f v = Vector3f(1.0f, 2.0f, 3.0f); v3(2) = 3.0f;
Vector3f v1; 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; unsigned int n = 60000;
@ -74,41 +90,51 @@ int test_mathlib(int argc, char *argv[])
t0 = hrt_absolute_time(); t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) { for (unsigned int j = 0; j < n; j++) {
v1 = m * v; vres3 = m3 * v3;
} }
t1 = hrt_absolute_time(); 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(); t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) { for (unsigned int j = 0; j < n; j++) {
mq1 = mq * mq; vres3old = m3old * v3old;
} }
t1 = hrt_absolute_time(); t1 = hrt_absolute_time();
warnx("Matrix * Matrix: %s %.6fus", formatResult(mq1 == mq), (double)(t1 - t0) / n); warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n);
mq1.dump();
t0 = hrt_absolute_time(); t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) { for (unsigned int j = 0; j < n; j++) {
m1 = m.transposed(); mres3 = m3 * m3;
} }
t1 = hrt_absolute_time(); 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(); t0 = hrt_absolute_time();
for (unsigned int j = 0; j < n; j++) { for (unsigned int j = 0; j < n; j++) {
m1 = m.inversed(); mres3old = m3old * m3old;
} }
t1 = hrt_absolute_time(); t1 = hrt_absolute_time();
warnx("Matrix Invert: %s %.6fus", formatResult(m1 == m), (double)(t1 - t0) / n); warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n);
Matrix<4,4> mn; t0 = hrt_absolute_time();
mn(0, 0) = 2.0f; for (unsigned int j = 0; j < n; j++) {
mn(1, 0) = 3.0f; mres4 = m4 * m4;
for (int i = 0; i < mn.getRows(); i++) { }
for (int j = 0; j < mn.getCols(); j++) { t1 = hrt_absolute_time();
printf("%.3f ", mn(i, j)); 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();
} }
printf("\n"); 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; return 0;
} }

Write Preview
Loading…
Cancel
Save