mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_Math/matrix3.h
This commit is contained in:
uncrustify
Pat Hickey
parent
9bb0b42f28
commit
7e19ae5868
|
|
@ -38,120 +38,159 @@ template <typename T>
|
|||
class Matrix3 {
|
||||
public:
|
||||
|
||||
// Vectors comprising the rows of the matrix
|
||||
Vector3<T> a, b, c;
|
||||
// Vectors comprising the rows of the matrix
|
||||
Vector3<T> a, b, c;
|
||||
|
||||
// trivial ctor
|
||||
// note that the Vector3 ctor will zero the vector elements
|
||||
Matrix3<T>() {}
|
||||
// trivial ctor
|
||||
// note that the Vector3 ctor will zero the vector elements
|
||||
Matrix3<T>() {
|
||||
}
|
||||
|
||||
// setting ctor
|
||||
Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0): a(a0), b(b0), c(c0) {}
|
||||
// setting ctor
|
||||
Matrix3<T>(const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0) : a(a0), b(b0), c(c0) {
|
||||
}
|
||||
|
||||
// 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): a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {}
|
||||
// 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) : a(ax,ay,az), b(bx,by,bz), c(cx,cy,cz) {
|
||||
}
|
||||
|
||||
// function call operator
|
||||
void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)
|
||||
{ a = a0; b = b0; c = c0; }
|
||||
// function call operator
|
||||
void operator () (const Vector3<T> a0, const Vector3<T> b0, const Vector3<T> c0)
|
||||
{
|
||||
a = a0; b = b0; c = c0;
|
||||
}
|
||||
|
||||
// test for equality
|
||||
bool operator == (const Matrix3<T> &m)
|
||||
{ return (a==m.a && b==m.b && c==m.c); }
|
||||
// test for equality
|
||||
bool operator == (const Matrix3<T> &m)
|
||||
{
|
||||
return (a==m.a && b==m.b && c==m.c);
|
||||
}
|
||||
|
||||
// test for inequality
|
||||
bool operator != (const Matrix3<T> &m)
|
||||
{ return (a!=m.a || b!=m.b || c!=m.c); }
|
||||
// test for inequality
|
||||
bool operator != (const Matrix3<T> &m)
|
||||
{
|
||||
return (a!=m.a || b!=m.b || c!=m.c);
|
||||
}
|
||||
|
||||
// negation
|
||||
Matrix3<T> operator - (void) const
|
||||
{ return Matrix3<T>(-a,-b,-c); }
|
||||
// negation
|
||||
Matrix3<T> operator - (void) const
|
||||
{
|
||||
return Matrix3<T>(-a,-b,-c);
|
||||
}
|
||||
|
||||
// addition
|
||||
Matrix3<T> operator + (const Matrix3<T> &m) const
|
||||
{ return Matrix3<T>(a+m.a, b+m.b, c+m.c); }
|
||||
Matrix3<T> &operator += (const Matrix3<T> &m)
|
||||
{ return *this = *this + m; }
|
||||
// addition
|
||||
Matrix3<T> operator + (const Matrix3<T> &m) const
|
||||
{
|
||||
return Matrix3<T>(a+m.a, b+m.b, c+m.c);
|
||||
}
|
||||
Matrix3<T> &operator += (const Matrix3<T> &m)
|
||||
{
|
||||
return *this = *this + m;
|
||||
}
|
||||
|
||||
// subtraction
|
||||
Matrix3<T> operator - (const Matrix3<T> &m) const
|
||||
{ return Matrix3<T>(a-m.a, b-m.b, c-m.c); }
|
||||
Matrix3<T> &operator -= (const Matrix3<T> &m)
|
||||
{ return *this = *this - m; }
|
||||
// subtraction
|
||||
Matrix3<T> operator - (const Matrix3<T> &m) const
|
||||
{
|
||||
return Matrix3<T>(a-m.a, b-m.b, c-m.c);
|
||||
}
|
||||
Matrix3<T> &operator -= (const Matrix3<T> &m)
|
||||
{
|
||||
return *this = *this - m;
|
||||
}
|
||||
|
||||
// uniform scaling
|
||||
Matrix3<T> operator * (const T num) const
|
||||
{ return Matrix3<T>(a*num, b*num, c*num); }
|
||||
Matrix3<T> &operator *= (const T num)
|
||||
{ return *this = *this * num; }
|
||||
Matrix3<T> operator / (const T num) const
|
||||
{ return Matrix3<T>(a/num, b/num, c/num); }
|
||||
Matrix3<T> &operator /= (const T num)
|
||||
{ return *this = *this / num; }
|
||||
// uniform scaling
|
||||
Matrix3<T> operator * (const T num) const
|
||||
{
|
||||
return Matrix3<T>(a*num, b*num, c*num);
|
||||
}
|
||||
Matrix3<T> &operator *= (const T num)
|
||||
{
|
||||
return *this = *this * num;
|
||||
}
|
||||
Matrix3<T> operator / (const T num) const
|
||||
{
|
||||
return Matrix3<T>(a/num, b/num, c/num);
|
||||
}
|
||||
Matrix3<T> &operator /= (const T num)
|
||||
{
|
||||
return *this = *this / num;
|
||||
}
|
||||
|
||||
// multiplication by a vector
|
||||
Vector3<T> operator *(const Vector3<T> &v) const;
|
||||
// multiplication by a vector
|
||||
Vector3<T> operator *(const Vector3<T> &v) const;
|
||||
|
||||
// multiplication of transpose by a vector
|
||||
Vector3<T> mul_transpose(const Vector3<T> &v) const;
|
||||
// multiplication of transpose by a vector
|
||||
Vector3<T> mul_transpose(const Vector3<T> &v) const;
|
||||
|
||||
// extract x column
|
||||
Vector3<T> colx(void) const
|
||||
{ return Vector3f(a.x, b.x, c.x); }
|
||||
// extract x column
|
||||
Vector3<T> colx(void) const
|
||||
{
|
||||
return Vector3f(a.x, b.x, c.x);
|
||||
}
|
||||
|
||||
// extract y column
|
||||
Vector3<T> coly(void) const
|
||||
{ return Vector3f(a.y, b.y, c.y); }
|
||||
// extract y column
|
||||
Vector3<T> coly(void) const
|
||||
{
|
||||
return Vector3f(a.y, b.y, c.y);
|
||||
}
|
||||
|
||||
// extract z column
|
||||
Vector3<T> colz(void) const
|
||||
{ return Vector3f(a.z, b.z, c.z); }
|
||||
// extract z column
|
||||
Vector3<T> colz(void) const
|
||||
{
|
||||
return Vector3f(a.z, b.z, c.z);
|
||||
}
|
||||
|
||||
// multiplication by another Matrix3<T>
|
||||
Matrix3<T> operator *(const Matrix3<T> &m) const;
|
||||
// multiplication by another Matrix3<T>
|
||||
Matrix3<T> operator *(const Matrix3<T> &m) const;
|
||||
|
||||
Matrix3<T> &operator *=(const Matrix3<T> &m)
|
||||
{ return *this = *this * m; }
|
||||
Matrix3<T> &operator *=(const Matrix3<T> &m)
|
||||
{
|
||||
return *this = *this * m;
|
||||
}
|
||||
|
||||
// transpose the matrix
|
||||
Matrix3<T> transposed(void) const;
|
||||
// transpose the matrix
|
||||
Matrix3<T> transposed(void) const;
|
||||
|
||||
Matrix3<T> transpose(void)
|
||||
{ return *this = transposed(); }
|
||||
Matrix3<T> transpose(void)
|
||||
{
|
||||
return *this = transposed();
|
||||
}
|
||||
|
||||
// zero the matrix
|
||||
void zero(void);
|
||||
// zero the matrix
|
||||
void zero(void);
|
||||
|
||||
// setup the identity matrix
|
||||
void identity(void) {
|
||||
a.x = b.y = c.z = 1;
|
||||
a.y = a.z = 0;
|
||||
b.x = b.z = 0;
|
||||
c.x = c.y = 0;
|
||||
}
|
||||
// setup the identity matrix
|
||||
void identity(void) {
|
||||
a.x = b.y = c.z = 1;
|
||||
a.y = a.z = 0;
|
||||
b.x = b.z = 0;
|
||||
c.x = c.y = 0;
|
||||
}
|
||||
|
||||
// check if any elements are NAN
|
||||
bool is_nan(void)
|
||||
{ return a.is_nan() || b.is_nan() || c.is_nan(); }
|
||||
// check if any elements are NAN
|
||||
bool is_nan(void)
|
||||
{
|
||||
return a.is_nan() || b.is_nan() || c.is_nan();
|
||||
}
|
||||
|
||||
// fill in the matrix with a standard rotation
|
||||
void rotation(enum Rotation rotation);
|
||||
// fill in the matrix with a standard rotation
|
||||
void rotation(enum Rotation rotation);
|
||||
|
||||
// create a rotation matrix from Euler angles
|
||||
void from_euler(float roll, float pitch, float yaw);
|
||||
void from_euler(float roll, float pitch, float yaw);
|
||||
|
||||
// create eulers from a rotation matrix
|
||||
void to_euler(float *roll, float *pitch, float *yaw);
|
||||
void to_euler(float *roll, float *pitch, float *yaw);
|
||||
|
||||
// apply an additional rotation from a body frame gyro vector
|
||||
// to a rotation matrix.
|
||||
void rotate(const Vector3<T> &g);
|
||||
void rotate(const Vector3<T> &g);
|
||||
};
|
||||
|
||||
typedef Matrix3<int16_t> Matrix3i;
|
||||
typedef Matrix3<uint16_t> Matrix3ui;
|
||||
typedef Matrix3<int32_t> Matrix3l;
|
||||
typedef Matrix3<uint32_t> Matrix3ul;
|
||||
typedef Matrix3<float> Matrix3f;
|
||||
typedef Matrix3<int16_t> Matrix3i;
|
||||
typedef Matrix3<uint16_t> Matrix3ui;
|
||||
typedef Matrix3<int32_t> Matrix3l;
|
||||
typedef Matrix3<uint32_t> Matrix3ul;
|
||||
typedef Matrix3<float> Matrix3f;
|
||||
|
||||
#endif // MATRIX3_H
|
||||
|
|
|
|||
Loading…
Reference in New Issue