forked from Archive/PX4-Autopilot
mathlib: code style fixed
This commit is contained in:
parent
8ed193d115
commit
bbeb97df67
|
@ -54,7 +54,8 @@ class __EXPORT Matrix;
|
||||||
|
|
||||||
// MxN matrix with float elements
|
// MxN matrix with float elements
|
||||||
template <unsigned int M, unsigned int N>
|
template <unsigned int M, unsigned int N>
|
||||||
class __EXPORT MatrixBase {
|
class __EXPORT MatrixBase
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* matrix data[row][col]
|
* matrix data[row][col]
|
||||||
|
@ -90,14 +91,14 @@ public:
|
||||||
/**
|
/**
|
||||||
* access by index
|
* 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];
|
return data[row][col];
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* access by index
|
* 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];
|
return data[row][col];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -123,6 +124,7 @@ public:
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
if (data[i][j] != m.data[i][j])
|
if (data[i][j] != m.data[i][j])
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -134,6 +136,7 @@ public:
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
if (data[i][j] != m.data[i][j])
|
if (data[i][j] != m.data[i][j])
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -150,9 +153,11 @@ public:
|
||||||
*/
|
*/
|
||||||
Matrix<M, N> operator -(void) const {
|
Matrix<M, N> operator -(void) const {
|
||||||
Matrix<M, N> res;
|
Matrix<M, N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
for (unsigned int j = 0; j < M; j++)
|
for (unsigned int j = 0; j < M; j++)
|
||||||
res.data[i][j] = -data[i][j];
|
res.data[i][j] = -data[i][j];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -161,9 +166,11 @@ public:
|
||||||
*/
|
*/
|
||||||
Matrix<M, N> operator +(const Matrix<M, N> &m) const {
|
Matrix<M, N> operator +(const Matrix<M, N> &m) const {
|
||||||
Matrix<M, N> res;
|
Matrix<M, N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
for (unsigned int j = 0; j < M; j++)
|
for (unsigned int j = 0; j < M; j++)
|
||||||
res.data[i][j] = data[i][j] + m.data[i][j];
|
res.data[i][j] = data[i][j] + m.data[i][j];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -171,6 +178,7 @@ public:
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
for (unsigned int j = 0; j < M; j++)
|
for (unsigned int j = 0; j < M; j++)
|
||||||
data[i][j] += m.data[i][j];
|
data[i][j] += m.data[i][j];
|
||||||
|
|
||||||
return *static_cast<Matrix<M, N>*>(this);
|
return *static_cast<Matrix<M, N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -179,9 +187,11 @@ public:
|
||||||
*/
|
*/
|
||||||
Matrix<M, N> operator -(const Matrix<M, N> &m) const {
|
Matrix<M, N> operator -(const Matrix<M, N> &m) const {
|
||||||
Matrix<M, N> res;
|
Matrix<M, N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < M; i++)
|
for (unsigned int i = 0; i < M; i++)
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
res.data[i][j] = data[i][j] - m.data[i][j];
|
res.data[i][j] = data[i][j] - m.data[i][j];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -189,6 +199,7 @@ public:
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
for (unsigned int j = 0; j < M; j++)
|
for (unsigned int j = 0; j < M; j++)
|
||||||
data[i][j] -= m.data[i][j];
|
data[i][j] -= m.data[i][j];
|
||||||
|
|
||||||
return *static_cast<Matrix<M, N>*>(this);
|
return *static_cast<Matrix<M, N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -197,9 +208,11 @@ public:
|
||||||
*/
|
*/
|
||||||
Matrix<M, N> operator *(const float num) const {
|
Matrix<M, N> operator *(const float num) const {
|
||||||
Matrix<M, N> res;
|
Matrix<M, N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < M; i++)
|
for (unsigned int i = 0; i < M; i++)
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
res.data[i][j] = data[i][j] * num;
|
res.data[i][j] = data[i][j] * num;
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -207,14 +220,17 @@ public:
|
||||||
for (unsigned int i = 0; i < M; i++)
|
for (unsigned int i = 0; i < M; i++)
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
data[i][j] *= num;
|
data[i][j] *= num;
|
||||||
|
|
||||||
return *static_cast<Matrix<M, N>*>(this);
|
return *static_cast<Matrix<M, N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
Matrix<M, N> operator /(const float num) const {
|
Matrix<M, N> operator /(const float num) const {
|
||||||
Matrix<M, N> res;
|
Matrix<M, N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < M; i++)
|
for (unsigned int i = 0; i < M; i++)
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
res[i][j] = data[i][j] / num;
|
res[i][j] = data[i][j] / num;
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -222,6 +238,7 @@ public:
|
||||||
for (unsigned int i = 0; i < M; i++)
|
for (unsigned int i = 0; i < M; i++)
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
data[i][j] /= num;
|
data[i][j] /= num;
|
||||||
|
|
||||||
return *static_cast<Matrix<M, N>*>(this);
|
return *static_cast<Matrix<M, N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -266,6 +283,7 @@ public:
|
||||||
void identity(void) {
|
void identity(void) {
|
||||||
memset(data, 0, sizeof(data));
|
memset(data, 0, sizeof(data));
|
||||||
unsigned int n = (M < N) ? M : N;
|
unsigned int n = (M < N) ? M : N;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < n; i++)
|
for (unsigned int i = 0; i < n; i++)
|
||||||
data[i][i] = 1;
|
data[i][i] = 1;
|
||||||
}
|
}
|
||||||
|
@ -273,15 +291,18 @@ public:
|
||||||
void print(void) {
|
void print(void) {
|
||||||
for (unsigned int i = 0; i < M; i++) {
|
for (unsigned int i = 0; i < M; i++) {
|
||||||
printf("[ ");
|
printf("[ ");
|
||||||
|
|
||||||
for (unsigned int j = 0; j < N; j++)
|
for (unsigned int j = 0; j < N; j++)
|
||||||
printf("%.3f\t", data[i][j]);
|
printf("%.3f\t", data[i][j]);
|
||||||
|
|
||||||
printf(" ]\n");
|
printf(" ]\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
template <unsigned int M, unsigned int N>
|
template <unsigned int M, unsigned int N>
|
||||||
class __EXPORT Matrix : public MatrixBase<M, N> {
|
class __EXPORT Matrix : public MatrixBase<M, N>
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
using MatrixBase<M, N>::operator *;
|
using MatrixBase<M, N>::operator *;
|
||||||
|
|
||||||
|
@ -310,7 +331,8 @@ public:
|
||||||
};
|
};
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> {
|
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
using MatrixBase<3, 3>::operator *;
|
using MatrixBase<3, 3>::operator *;
|
||||||
|
|
||||||
|
@ -380,6 +402,7 @@ public:
|
||||||
euler.data[0] = atan2f(data[2][1], data[2][2]);
|
euler.data[0] = atan2f(data[2][1], data[2][2]);
|
||||||
euler.data[2] = atan2f(data[1][0], data[0][0]);
|
euler.data[2] = atan2f(data[1][0], data[0][0]);
|
||||||
}
|
}
|
||||||
|
|
||||||
return euler;
|
return euler;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
|
@ -51,7 +51,8 @@
|
||||||
namespace math
|
namespace math
|
||||||
{
|
{
|
||||||
|
|
||||||
class __EXPORT Quaternion : public Vector<4> {
|
class __EXPORT Quaternion : public Vector<4>
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* trivial ctor
|
* trivial ctor
|
||||||
|
@ -101,7 +102,7 @@ public:
|
||||||
data[2], data[3], data[0], -data[1],
|
data[2], data[3], data[0], -data[1],
|
||||||
data[3], -data[2], data[1], data[0]
|
data[3], -data[2], data[1], data[0]
|
||||||
};
|
};
|
||||||
Matrix<4,4> Q(dataQ);
|
Matrix<4, 4> Q(dataQ);
|
||||||
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
|
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
|
||||||
return Q * v * 0.5f;
|
return Q * v * 0.5f;
|
||||||
}
|
}
|
||||||
|
|
|
@ -54,7 +54,8 @@ template <unsigned int N>
|
||||||
class __EXPORT Vector;
|
class __EXPORT Vector;
|
||||||
|
|
||||||
template <unsigned int N>
|
template <unsigned int N>
|
||||||
class __EXPORT VectorBase {
|
class __EXPORT VectorBase
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* vector data
|
* vector data
|
||||||
|
@ -93,14 +94,14 @@ public:
|
||||||
/**
|
/**
|
||||||
* access to elements by index
|
* access to elements by index
|
||||||
*/
|
*/
|
||||||
float &operator ()(const unsigned int i) {
|
float &operator()(const unsigned int i) {
|
||||||
return data[i];
|
return data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* access to elements by index
|
* access to elements by index
|
||||||
*/
|
*/
|
||||||
float operator ()(const unsigned int i) const {
|
float operator()(const unsigned int i) const {
|
||||||
return data[i];
|
return data[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -118,6 +119,7 @@ public:
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
if (data[i] != v.data[i])
|
if (data[i] != v.data[i])
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -128,6 +130,7 @@ public:
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
if (data[i] != v.data[i])
|
if (data[i] != v.data[i])
|
||||||
return true;
|
return true;
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -144,8 +147,10 @@ public:
|
||||||
*/
|
*/
|
||||||
const Vector<N> operator -(void) const {
|
const Vector<N> operator -(void) const {
|
||||||
Vector<N> res;
|
Vector<N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res.data[i] = -data[i];
|
res.data[i] = -data[i];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -154,8 +159,10 @@ public:
|
||||||
*/
|
*/
|
||||||
const Vector<N> operator +(const Vector<N> &v) const {
|
const Vector<N> operator +(const Vector<N> &v) const {
|
||||||
Vector<N> res;
|
Vector<N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res.data[i] = data[i] + v.data[i];
|
res.data[i] = data[i] + v.data[i];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -164,8 +171,10 @@ public:
|
||||||
*/
|
*/
|
||||||
const Vector<N> operator -(const Vector<N> &v) const {
|
const Vector<N> operator -(const Vector<N> &v) const {
|
||||||
Vector<N> res;
|
Vector<N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res.data[i] = data[i] - v.data[i];
|
res.data[i] = data[i] - v.data[i];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -174,8 +183,10 @@ public:
|
||||||
*/
|
*/
|
||||||
const Vector<N> operator *(const float num) const {
|
const Vector<N> operator *(const float num) const {
|
||||||
Vector<N> res;
|
Vector<N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res.data[i] = data[i] * num;
|
res.data[i] = data[i] * num;
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -184,8 +195,10 @@ public:
|
||||||
*/
|
*/
|
||||||
const Vector<N> operator /(const float num) const {
|
const Vector<N> operator /(const float num) const {
|
||||||
Vector<N> res;
|
Vector<N> res;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res.data[i] = data[i] / num;
|
res.data[i] = data[i] / num;
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -195,6 +208,7 @@ public:
|
||||||
const Vector<N> &operator +=(const Vector<N> &v) {
|
const Vector<N> &operator +=(const Vector<N> &v) {
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
data[i] += v.data[i];
|
data[i] += v.data[i];
|
||||||
|
|
||||||
return *static_cast<const Vector<N>*>(this);
|
return *static_cast<const Vector<N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -204,6 +218,7 @@ public:
|
||||||
const Vector<N> &operator -=(const Vector<N> &v) {
|
const Vector<N> &operator -=(const Vector<N> &v) {
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
data[i] -= v.data[i];
|
data[i] -= v.data[i];
|
||||||
|
|
||||||
return *static_cast<const Vector<N>*>(this);
|
return *static_cast<const Vector<N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -213,6 +228,7 @@ public:
|
||||||
const Vector<N> &operator *=(const float num) {
|
const Vector<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 *static_cast<const Vector<N>*>(this);
|
return *static_cast<const Vector<N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -222,6 +238,7 @@ public:
|
||||||
const Vector<N> &operator /=(const float num) {
|
const Vector<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 *static_cast<const Vector<N>*>(this);
|
return *static_cast<const Vector<N>*>(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -230,8 +247,10 @@ public:
|
||||||
*/
|
*/
|
||||||
float operator *(const Vector<N> &v) const {
|
float operator *(const Vector<N> &v) const {
|
||||||
float res = 0.0f;
|
float res = 0.0f;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res += data[i] * v.data[i];
|
res += data[i] * v.data[i];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -240,8 +259,10 @@ public:
|
||||||
*/
|
*/
|
||||||
float length_squared() const {
|
float length_squared() const {
|
||||||
float res = 0.0f;
|
float res = 0.0f;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res += data[i] * data[i];
|
res += data[i] * data[i];
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -250,8 +271,10 @@ public:
|
||||||
*/
|
*/
|
||||||
float length() const {
|
float length() const {
|
||||||
float res = 0.0f;
|
float res = 0.0f;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
res += data[i] * data[i];
|
res += data[i] * data[i];
|
||||||
|
|
||||||
return sqrtf(res);
|
return sqrtf(res);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -278,14 +301,17 @@ public:
|
||||||
|
|
||||||
void print(void) {
|
void print(void) {
|
||||||
printf("[ ");
|
printf("[ ");
|
||||||
|
|
||||||
for (unsigned int i = 0; i < N; i++)
|
for (unsigned int i = 0; i < N; i++)
|
||||||
printf("%.3f\t", data[i]);
|
printf("%.3f\t", data[i]);
|
||||||
|
|
||||||
printf("]\n");
|
printf("]\n");
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
template <unsigned int N>
|
template <unsigned int N>
|
||||||
class __EXPORT Vector : public VectorBase<N> {
|
class __EXPORT Vector : public VectorBase<N>
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
Vector() : VectorBase<N>() {}
|
Vector() : VectorBase<N>() {}
|
||||||
|
|
||||||
|
@ -303,7 +329,8 @@ public:
|
||||||
};
|
};
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
class __EXPORT Vector<2> : public VectorBase<2> {
|
class __EXPORT Vector<2> : public VectorBase<2>
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
Vector() : VectorBase<2>() {}
|
Vector() : VectorBase<2>() {}
|
||||||
|
|
||||||
|
@ -338,7 +365,8 @@ public:
|
||||||
};
|
};
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
class __EXPORT Vector<3> : public VectorBase<3> {
|
class __EXPORT Vector<3> : public VectorBase<3>
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
Vector() : VectorBase<3>() {}
|
Vector() : VectorBase<3>() {}
|
||||||
|
|
||||||
|
@ -365,6 +393,7 @@ public:
|
||||||
const Vector<3> &operator =(const Vector<3> &v) {
|
const Vector<3> &operator =(const Vector<3> &v) {
|
||||||
for (unsigned int i = 0; i < 3; i++)
|
for (unsigned int i = 0; i < 3; i++)
|
||||||
data[i] = v.data[i];
|
data[i] = v.data[i];
|
||||||
|
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -378,7 +407,8 @@ public:
|
||||||
};
|
};
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
class __EXPORT Vector<4> : public VectorBase<4> {
|
class __EXPORT Vector<4> : public VectorBase<4>
|
||||||
|
{
|
||||||
public:
|
public:
|
||||||
Vector() : VectorBase() {}
|
Vector() : VectorBase() {}
|
||||||
|
|
||||||
|
@ -405,6 +435,7 @@ public:
|
||||||
const Vector<4> &operator =(const Vector<4> &v) {
|
const Vector<4> &operator =(const Vector<4> &v) {
|
||||||
for (unsigned int i = 0; i < 4; i++)
|
for (unsigned int i = 0; i < 4; i++)
|
||||||
data[i] = v.data[i];
|
data[i] = v.data[i];
|
||||||
|
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue