fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 04:53:34 +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