diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 42fe8e9faf..8f76a5b08f 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -38,120 +38,159 @@ template class Matrix3 { public: - // Vectors comprising the rows of the matrix - Vector3 a, b, c; + // Vectors comprising the rows of the matrix + Vector3 a, b, c; - // trivial ctor - // note that the Vector3 ctor will zero the vector elements - Matrix3() {} + // trivial ctor + // note that the Vector3 ctor will zero the vector elements + Matrix3() { + } - // setting ctor - Matrix3(const Vector3 a0, const Vector3 b0, const Vector3 c0): a(a0), b(b0), c(c0) {} + // setting ctor + Matrix3(const Vector3 a0, const Vector3 b0, const Vector3 c0) : a(a0), b(b0), c(c0) { + } - // setting ctor - Matrix3(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(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 a0, const Vector3 b0, const Vector3 c0) - { a = a0; b = b0; c = c0; } + // function call operator + void operator () (const Vector3 a0, const Vector3 b0, const Vector3 c0) + { + a = a0; b = b0; c = c0; + } - // test for equality - bool operator == (const Matrix3 &m) - { return (a==m.a && b==m.b && c==m.c); } + // test for equality + bool operator == (const Matrix3 &m) + { + return (a==m.a && b==m.b && c==m.c); + } - // test for inequality - bool operator != (const Matrix3 &m) - { return (a!=m.a || b!=m.b || c!=m.c); } + // test for inequality + bool operator != (const Matrix3 &m) + { + return (a!=m.a || b!=m.b || c!=m.c); + } - // negation - Matrix3 operator - (void) const - { return Matrix3(-a,-b,-c); } + // negation + Matrix3 operator - (void) const + { + return Matrix3(-a,-b,-c); + } - // addition - Matrix3 operator + (const Matrix3 &m) const - { return Matrix3(a+m.a, b+m.b, c+m.c); } - Matrix3 &operator += (const Matrix3 &m) - { return *this = *this + m; } + // addition + Matrix3 operator + (const Matrix3 &m) const + { + return Matrix3(a+m.a, b+m.b, c+m.c); + } + Matrix3 &operator += (const Matrix3 &m) + { + return *this = *this + m; + } - // subtraction - Matrix3 operator - (const Matrix3 &m) const - { return Matrix3(a-m.a, b-m.b, c-m.c); } - Matrix3 &operator -= (const Matrix3 &m) - { return *this = *this - m; } + // subtraction + Matrix3 operator - (const Matrix3 &m) const + { + return Matrix3(a-m.a, b-m.b, c-m.c); + } + Matrix3 &operator -= (const Matrix3 &m) + { + return *this = *this - m; + } - // uniform scaling - Matrix3 operator * (const T num) const - { return Matrix3(a*num, b*num, c*num); } - Matrix3 &operator *= (const T num) - { return *this = *this * num; } - Matrix3 operator / (const T num) const - { return Matrix3(a/num, b/num, c/num); } - Matrix3 &operator /= (const T num) - { return *this = *this / num; } + // uniform scaling + Matrix3 operator * (const T num) const + { + return Matrix3(a*num, b*num, c*num); + } + Matrix3 &operator *= (const T num) + { + return *this = *this * num; + } + Matrix3 operator / (const T num) const + { + return Matrix3(a/num, b/num, c/num); + } + Matrix3 &operator /= (const T num) + { + return *this = *this / num; + } - // multiplication by a vector - Vector3 operator *(const Vector3 &v) const; + // multiplication by a vector + Vector3 operator *(const Vector3 &v) const; - // multiplication of transpose by a vector - Vector3 mul_transpose(const Vector3 &v) const; + // multiplication of transpose by a vector + Vector3 mul_transpose(const Vector3 &v) const; - // extract x column - Vector3 colx(void) const - { return Vector3f(a.x, b.x, c.x); } + // extract x column + Vector3 colx(void) const + { + return Vector3f(a.x, b.x, c.x); + } - // extract y column - Vector3 coly(void) const - { return Vector3f(a.y, b.y, c.y); } + // extract y column + Vector3 coly(void) const + { + return Vector3f(a.y, b.y, c.y); + } - // extract z column - Vector3 colz(void) const - { return Vector3f(a.z, b.z, c.z); } + // extract z column + Vector3 colz(void) const + { + return Vector3f(a.z, b.z, c.z); + } - // multiplication by another Matrix3 - Matrix3 operator *(const Matrix3 &m) const; + // multiplication by another Matrix3 + Matrix3 operator *(const Matrix3 &m) const; - Matrix3 &operator *=(const Matrix3 &m) - { return *this = *this * m; } + Matrix3 &operator *=(const Matrix3 &m) + { + return *this = *this * m; + } - // transpose the matrix - Matrix3 transposed(void) const; + // transpose the matrix + Matrix3 transposed(void) const; - Matrix3 transpose(void) - { return *this = transposed(); } + Matrix3 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 &g); + void rotate(const Vector3 &g); }; -typedef Matrix3 Matrix3i; -typedef Matrix3 Matrix3ui; -typedef Matrix3 Matrix3l; -typedef Matrix3 Matrix3ul; -typedef Matrix3 Matrix3f; +typedef Matrix3 Matrix3i; +typedef Matrix3 Matrix3ui; +typedef Matrix3 Matrix3l; +typedef Matrix3 Matrix3ul; +typedef Matrix3 Matrix3f; #endif // MATRIX3_H