fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 18:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib: fixes and improvements, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-18 23:01:02 +04:00
parent a83e3cd222
commit e3a5a384d7
4 changed files with 156 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Matrix.hpp
+27 -11
View File
@@ -34,9 +34,9 @@
****************************************************************************/
/**
* @file Matrix3.hpp
* @file Matrix.hpp
*
* 3x3 Matrix
* Generic Matrix
*/
#ifndef MATRIX_HPP
@@ -203,6 +203,24 @@ public:
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
*/
@@ -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]);
}
*/
};
}
src/lib/mathlib/math/Matrix3.hpp
+10 -2
View File
@@ -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]};
}
/**
@@ -124,6 +124,14 @@ public:
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
*/
src/lib/mathlib/math/Vector.hpp
+67 -23
View File
@@ -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
src/systemcmds/tests/test_mathlib.cpp
+52 -26
View File
@@ -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);
warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (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");
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;
}