/* 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 // #pragma once #include "ftype.h" #include "vector3.h" #include "vector2.h" template class Vector3; // 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 constexpr Matrix3() {} // setting ctor constexpr Matrix3(const Vector3 &a0, const Vector3 &b0, const Vector3 &c0) : a(a0) , b(b0) , c(c0) {} // setting ctor constexpr 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 MATH_CHECK_INDEXES assert(i >= 0 && i < 3); #endif return _v[i]; } const Vector3 & operator[](uint8_t i) const { const Vector3 *_v = &a; #if MATH_CHECK_INDEXES 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(); } /** * Calculate the determinant of this matrix. * * @return The value of the determinant. */ T det() const; /** * Calculate the inverse of this matrix. * * @param inv[in] Where to store the result. * * @return If this matrix is invertible, then true is returned. Otherwise, * \p inv is unmodified and false is returned. */ bool inverse(Matrix3& inv) const WARN_IF_UNUSED; /** * Invert this matrix if it is invertible. * * @return Return true if this matrix could be successfully inverted and * false otherwise. */ bool invert() WARN_IF_UNUSED; // zero the matrix void zero(void) { memset((void*)this, 0, sizeof(*this)); } // setup the identity matrix void identity(void) { zero(); a.x = b.y = c.z = 1; } // check if any elements are NAN bool is_nan(void) WARN_IF_UNUSED { return a.is_nan() || b.is_nan() || c.is_nan(); } /* create a rotation matrix from Euler angles in 321 euler ordering */ void from_euler(T roll, T pitch, T yaw); /* create eulers from a rotation matrix. roll is from -Pi to Pi pitch is from -Pi/2 to Pi/2 yaw is from -Pi to Pi euler order is 321 */ void to_euler(T *roll, T *pitch, T *yaw) const; // create matrix from rotation enum void from_rotation(enum Rotation rotation); /* calculate Euler angles (312 convention) for the matrix. See http://www.atacolorado.com/eulersequences.doc vector is returned in r, p, y order */ Vector3 to_euler312() const; /* fill the matrix from Euler angles in radians in 312 convention */ void from_euler312(T roll, T pitch, T yaw); // apply an additional rotation from a body frame gyro vector // to a rotation matrix. void rotate(const Vector3 &g); // create rotation matrix for rotation about the vector v by angle theta // See: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations // "Rotation matrix from axis and angle" void from_axis_angle(const Vector3 &v, T theta); // normalize a rotation matrix void normalize(void); // double/float conversion Matrix3 todouble(void) const { return Matrix3(a.todouble(), b.todouble(), c.todouble()); } Matrix3 tofloat(void) const { return Matrix3(a.tofloat(), b.tofloat(), c.tofloat()); } }; typedef Matrix3 Matrix3i; typedef Matrix3 Matrix3ui; typedef Matrix3 Matrix3l; typedef Matrix3 Matrix3ul; typedef Matrix3 Matrix3f; typedef Matrix3 Matrix3d;