// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ // Copyright 2010 Michael Smith, all rights reserved. // Inspired by: /**************************************** * 3D Vector Classes * By Bill Perone (billperone@yahoo.com) */ // // 3x3 matrix implementation. // // Note that the matrix is organised in row-normal form (the same as // applies to array indexing). // // In addition to the template, this header defines the following types: // // Matrix3i 3x3 matrix of signed integers // Matrix3ui 3x3 matrix of unsigned integers // Matrix3l 3x3 matrix of signed longs // Matrix3ul 3x3 matrix of unsigned longs // Matrix3f 3x3 matrix of signed floats // #ifndef MATRIX3_H #define MATRIX3_H #include "vector3.h" // 3x3 matrix with elements of type T template class Matrix3 { public: // Vectors comprising the rows of the matrix Vector3 a, b, c; // 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 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; } // 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); } // 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; } // 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; } // allow a Matrix3 to be used as an array of vectors, 0 indexed Vector3 & operator[](uint8_t i) { Vector3 *_v = &a; #if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1) assert(i >= 0 && i < 3); #endif return _v[i]; } const Vector3 & operator[](uint8_t i) const { const Vector3 *_v = &a; #if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1) assert(i >= 0 && i < 3); #endif return _v[i]; } // multiplication by a vector Vector3 operator *(const Vector3 &v) const; // multiplication of transpose by a vector Vector3 mul_transpose(const Vector3 &v) const; // multiplication by a vector giving a Vector2 result (XY components) Vector2 mulXY(const Vector3 &v) const; // extract x column Vector3 colx(void) const { return Vector3(a.x, b.x, c.x); } // extract y column Vector3 coly(void) const { return Vector3(a.y, b.y, c.y); } // extract z column Vector3 colz(void) const { return Vector3(a.z, b.z, c.z); } // multiplication by another Matrix3 Matrix3 operator *(const Matrix3 &m) const; Matrix3 &operator *=(const Matrix3 &m) { return *this = *this * m; } // transpose the matrix Matrix3 transposed(void) const; void transpose(void) { *this = transposed(); } // 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; } // check if any elements are NAN bool is_nan(void) { return a.is_nan() || b.is_nan() || c.is_nan(); } // create a rotation matrix from Euler angles void from_euler(float roll, float pitch, float yaw); // create eulers from a rotation matrix void to_euler(float *roll, float *pitch, float *yaw) const; // apply an additional rotation from a body frame gyro vector // to a rotation matrix. void rotate(const Vector3 &g); // apply an additional rotation from a body frame gyro vector // to a rotation matrix but only use X, Y elements from gyro vector void rotateXY(const Vector3 &g); // apply an additional inverse rotation to a rotation matrix but // only use X, Y elements from rotation vector void rotateXYinv(const Vector3 &g); // normalize a rotation matrix void normalize(void); }; typedef Matrix3 Matrix3i; typedef Matrix3 Matrix3ui; typedef Matrix3 Matrix3l; typedef Matrix3 Matrix3ul; typedef Matrix3 Matrix3f; #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 typedef Matrix3 Matrix3d; #endif #endif // MATRIX3_H