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