diff --git a/libs/main/math/pimathmatrix.h b/libs/main/math/pimathmatrix.h index 645c9e22..1e263e15 100644 --- a/libs/main/math/pimathmatrix.h +++ b/libs/main/math/pimathmatrix.h @@ -31,12 +31,9 @@ /// Matrix templated -#define PIMM_FOR(r, c) for (uint c = 0; c < Cols; ++c) { for (uint r = 0; r < Rows; ++r) { -#define PIMM_FOR_WB(r, c) for (uint c = 0; c < Cols; ++c) for (uint r = 0; r < Rows; ++r) // without brakes -#define PIMM_FOR_I(r, c) for (uint r = 0; r < Rows; ++r) { for (uint c = 0; c < Cols; ++c) { -#define PIMM_FOR_I_WB(r, c) for (uint r = 0; r < Rows; ++r) for (uint c = 0; c < Cols; ++c) // without brakes -#define PIMM_FOR_C(v) for (uint v = 0; v < Cols; ++v) -#define PIMM_FOR_R(v) for (uint v = 0; v < Rows; ++v) +#define PIMM_FOR for (uint r = 0; r < Rows; ++r) for (uint c = 0; c < Cols; ++c) +#define PIMM_FOR_C for (uint i = 0; i < Cols; ++i) +#define PIMM_FOR_R for (uint i = 0; i < Rows; ++i) #pragma pack(push, 1) @@ -71,7 +68,16 @@ public: PIMathMatrixT(const PIVector &val) { resize(Rows, Cols); int i = 0; - PIMM_FOR_I_WB(r, c) m[r][c] = val[i++]; + PIMM_FOR m[r][c] = val[i++]; + } + + /** + * @brief Contructs PIMathMatrixT from C++11 initializer list + */ + PIMathMatrixT(std::initializer_list init_list) { + assert(Rows*Cols == init_list.size()); + int i = 0; + PIMM_FOR m[r][c] = init_list.begin()[i++]; } /** @@ -81,7 +87,7 @@ public: */ static _CMatrix identity() { _CMatrix tm = _CMatrix(); - PIMM_FOR_WB(r, c) tm.m[r][c] = (c == r ? Type(1) : Type(0)); + PIMM_FOR tm.m[r][c] = (c == r ? Type(1) : Type(0)); return tm; } @@ -93,73 +99,10 @@ public: */ static _CMatrix filled(const Type &v) { _CMatrix tm; - PIMM_FOR_WB(r, c) tm.m[r][c] = v; + PIMM_FOR tm.m[r][c] = v; return tm; } - /** - * @brief Rotation the matrix by an "angle". Works only with 2x2 matrix, - * else return default construction of PIMathMatrixT - * - * @param angle is the angle of rotation of the matrix - * @return rotated matrix - */ - static _CMatrix rotation(double angle) { return _CMatrix(); } - - /** - * @brief Rotation of the matrix by an "angle" along the X axis. Works only with 3x3 matrix, - * else return default construction of PIMathMatrixT - * - * @param angle is the angle of rotation of the matrix along the X axis - * @return rotated matrix - */ - static _CMatrix rotationX(double angle) { return _CMatrix(); } - - /** - * @brief Rotation of the matrix by an "angle" along the Y axis. Works only with 3x3 matrix, - * else return default construction of PIMathMatrixT - * - * @param angle is the angle of rotation of the matrix along the Y axis - * @return rotated matrix - */ - static _CMatrix rotationY(double angle) { return _CMatrix(); } - - /** - * @brief Rotation of the matrix by an "angle" along the Z axis. Works only with 3x3 matrix, - * else return default construction of PIMathMatrixT - * - * @param angle is the angle of rotation of the matrix along the Z axis - * @return rotated matrix - */ - static _CMatrix rotationZ(double angle) { return _CMatrix(); } - - /** - * @brief Scaling the matrix along the X axis by the value "factor". Works only with 3x3 and 2x2 matrix, - * else return default construction of PIMathMatrixT - * - * @param factor is the value of scaling by X axis - * @return rotated matrix - */ - static _CMatrix scaleX(double factor) { return _CMatrix(); } - - /** - * @brief Scaling the matrix along the Y axis by the value "factor". Works only with 3x3 and 2x2 matrix, - * else return default construction of PIMathMatrixT - * - * @param factor is the value of scaling by Y axis - * @return rotated matrix - */ - static _CMatrix scaleY(double factor) { return _CMatrix(); } - - /** - * @brief Scaling the matrix along the Z axis by the value "factor". Works only with 3x3 matrix, - * else return default construction of PIMathMatrixT - * - * @param factor is the value of scaling by Z axis - * @return rotated matrix - */ - static _CMatrix scaleZ(double factor) { return _CMatrix(); } - /** * @brief Method which returns number of columns in matrix * @@ -183,7 +126,7 @@ public: */ _CMCol col(uint index) { _CMCol tv; - PIMM_FOR_R(i) tv[i] = m[i][index]; + PIMM_FOR_R tv[i] = m[i][index]; return tv; } @@ -196,7 +139,7 @@ public: */ _CMRow row(uint index) { _CMRow tv; - PIMM_FOR_C(i) tv[i] = m[index][i]; + PIMM_FOR_C tv[i] = m[index][i]; return tv; } @@ -209,7 +152,7 @@ public: * @return matrix type _CMatrix */ _CMatrix &setCol(uint index, const _CMCol &v) { - PIMM_FOR_R(i) m[i][index] = v[i]; + PIMM_FOR_R m[i][index] = v[i]; return *this; } @@ -222,7 +165,7 @@ public: * @return matrix type _CMatrix */ _CMatrix &setRow(uint index, const _CMRow &v) { - PIMM_FOR_C(i) m[index][i] = v[i]; + PIMM_FOR_C m[index][i] = v[i]; return *this; } @@ -236,7 +179,7 @@ public: */ _CMatrix &swapRows(uint r0, uint r1) { Type t; - PIMM_FOR_C(i) { + PIMM_FOR_C { t = m[r0][i]; m[r0][i] = m[r1][i]; m[r1][i] = t; @@ -254,7 +197,7 @@ public: */ _CMatrix &swapCols(uint c0, uint c1) { Type t; - PIMM_FOR_R(i) { + PIMM_FOR_R { t = m[i][c0]; m[i][c0] = m[i][c1]; m[i][c1] = t; @@ -269,7 +212,7 @@ public: * @return filled matrix type _CMatrix */ _CMatrix &fill(const Type &v) { - PIMM_FOR_WB(r, c) m[r][c] = v; + PIMM_FOR m[r][c] = v; return *this; } @@ -286,7 +229,7 @@ public: * @return true if matrix is identitied, else false */ bool isIdentity() const { - PIMM_FOR_WB(r, c) if ((c == r) ? m[r][c] != Type(1) : m[r][c] != Type(0)) return false; + PIMM_FOR if ((c == r) ? m[r][c] != Type(1) : m[r][c] != Type(0)) return false; return true; } @@ -296,7 +239,7 @@ public: * @return true if matrix is null, else false */ bool isNull() const { - PIMM_FOR_WB(r, c) if (m[r][c] != Type(0)) return false; + PIMM_FOR if (m[r][c] != Type(0)) return false; return true; } @@ -354,7 +297,7 @@ public: * @return if matrices are equal true, else false */ bool operator==(const _CMatrix &sm) const { - PIMM_FOR_WB(r, c) if (m[r][c] != sm.m[r][c]) return false; + PIMM_FOR if (m[r][c] != sm.m[r][c]) return false; return true; } @@ -371,28 +314,28 @@ public: * * @param sm matrix for the addition assigment */ - void operator+=(const _CMatrix &sm) { PIMM_FOR_WB(r, c) m[r][c] += sm.m[r][c]; } + void operator+=(const _CMatrix &sm) { PIMM_FOR m[r][c] += sm.m[r][c]; } /** * @brief Subtraction assignment with matrix "sm" * * @param sm matrix for the subtraction assigment */ - void operator-=(const _CMatrix &sm) { PIMM_FOR_WB(r, c) m[r][c] -= sm.m[r][c]; } + void operator-=(const _CMatrix &sm) { PIMM_FOR m[r][c] -= sm.m[r][c]; } /** * @brief Multiplication assignment with value "v" * * @param v value for the multiplication assigment */ - void operator*=(const Type &v) { PIMM_FOR_WB(r, c) m[r][c] *= v; } + void operator*=(const Type &v) { PIMM_FOR m[r][c] *= v; } /** * @brief Division assignment with value "v" * * @param v value for the division assigment */ - void operator/=(const Type &v) { PIMM_FOR_WB(r, c) m[r][c] /= v; } + void operator/=(const Type &v) { PIMM_FOR m[r][c] /= v; } /** * @brief Matrix substraction @@ -401,7 +344,7 @@ public: */ _CMatrix operator-() const { _CMatrix tm; - PIMM_FOR_WB(r, c) tm.m[r][c] = -m[r][c]; + PIMM_FOR tm.m[r][c] = -m[r][c]; return tm; } @@ -413,7 +356,7 @@ public: */ _CMatrix operator+(const _CMatrix &sm) const { _CMatrix tm = _CMatrix(*this); - PIMM_FOR_WB(r, c) tm.m[r][c] += sm.m[r][c]; + PIMM_FOR tm.m[r][c] += sm.m[r][c]; return tm; } @@ -425,7 +368,7 @@ public: */ _CMatrix operator-(const _CMatrix &sm) const { _CMatrix tm = _CMatrix(*this); - PIMM_FOR_WB(r, c) tm.m[r][c] -= sm.m[r][c]; + PIMM_FOR tm.m[r][c] -= sm.m[r][c]; return tm; } @@ -437,7 +380,7 @@ public: */ _CMatrix operator*(const Type &v) const { _CMatrix tm = _CMatrix(*this); - PIMM_FOR_WB(r, c) tm.m[r][c] *= v; + PIMM_FOR tm.m[r][c] *= v; return tm; } @@ -449,7 +392,7 @@ public: */ _CMatrix operator/(const Type &v) const { _CMatrix tm = _CMatrix(*this); - PIMM_FOR_WB(r, c) tm.m[r][c] /= v; + PIMM_FOR tm.m[r][c] /= v; return tm; } @@ -467,10 +410,7 @@ public: if (ok) *ok = k; if (!k) return ret; ret = Type(1); - for (uint c = 0; c < Cols; ++c) - for (uint r = 0; r < Rows; ++r) - if (r == c) - ret *= m[r][c]; + PIMM_FOR if (r == c) ret *= m[r][c]; return ret; } @@ -590,7 +530,7 @@ public: */ _CMatrixI transposed() const { _CMatrixI tm; - PIMM_FOR_WB(r, c) tm[c][r] = m[r][c]; + PIMM_FOR tm[c][r] = m[r][c]; return tm; } @@ -598,7 +538,7 @@ private: void resize(uint rows_, uint cols_, const Type &new_value = Type()) { r_ = rows_; c_ = cols_; - PIMM_FOR_WB(r, c) m[r][c] = new_value; + PIMM_FOR m[r][c] = new_value; } int c_, r_; @@ -609,92 +549,22 @@ private: #pragma pack(pop) -template<> -inline PIMathMatrixT<2u, 2u> PIMathMatrixT<2u, 2u>::rotation(double angle) { - double c = cos(angle), s = sin(angle); - PIMathMatrixT<2u, 2u> tm; - tm[0][0] = tm[1][1] = c; - tm[0][1] = -s; - tm[1][0] = s; - return tm; -} -template<> -inline PIMathMatrixT<2u, 2u> PIMathMatrixT<2u, 2u>::scaleX(double factor) { - PIMathMatrixT<2u, 2u> tm; - tm[0][0] = factor; - tm[1][1] = 1.; - return tm; -} - -template<> -inline PIMathMatrixT<2u, 2u> PIMathMatrixT<2u, 2u>::scaleY(double factor) { - PIMathMatrixT<2u, 2u> tm; - tm[0][0] = 1.; - tm[1][1] = factor; - return tm; -} - -template<> -inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::rotationX(double angle) { - double c = cos(angle), s = sin(angle); - PIMathMatrixT<3u, 3u> tm; - tm[0][0] = 1.; - tm[1][1] = tm[2][2] = c; - tm[2][1] = s; - tm[1][2] = -s; - return tm; -} - -template<> -inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::rotationY(double angle) { - double c = cos(angle), s = sin(angle); - PIMathMatrixT<3u, 3u> tm; - tm[1][1] = 1.; - tm[0][0] = tm[2][2] = c; - tm[2][0] = -s; - tm[0][2] = s; - return tm; -} - -template<> -inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::rotationZ(double angle) { - double c = cos(angle), s = sin(angle); - PIMathMatrixT<3u, 3u> tm; - tm[2][2] = 1.; - tm[0][0] = tm[1][1] = c; - tm[1][0] = s; - tm[0][1] = -s; - return tm; -} - -template<> -inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::scaleX(double factor) { - PIMathMatrixT<3u, 3u> tm; - tm[1][1] = tm[2][2] = 1.; - tm[0][0] = factor; - return tm; -} - -template<> -inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::scaleY(double factor) { - PIMathMatrixT<3u, 3u> tm; - tm[0][0] = tm[2][2] = 1.; - tm[1][1] = factor; - return tm; -} - -template<> -inline PIMathMatrixT<3u, 3u> PIMathMatrixT<3u, 3u>::scaleZ(double factor) { - PIMathMatrixT<3u, 3u> tm; - tm[0][0] = tm[1][1] = 1.; - tm[2][2] = factor; - return tm; -} #ifdef PIP_STD_IOSTREAM template -inline std::ostream & operator <<(std::ostream & s, const PIMathMatrixT & m) {s << "{"; PIMM_FOR_I(r, c) s << m[r][c]; if (c < Cols - 1 || r < Rows - 1) s << ", ";} if (r < Rows - 1) s << std::endl << " ";} s << "}"; return s;} +inline std::ostream & operator <<(std::ostream & s, const PIMathMatrixT & m) { + s << "{"; + for (uint r = 0; r < Rows; ++r) { + for (uint c = 0; c < Cols; ++c) { + s << m[r][c]; + if (c < Cols - 1 || r < Rows - 1) s << ", "; + } + if (r < Rows - 1) s << std::endl << " "; + } + s << "}"; + return s; +} #endif /** @@ -707,9 +577,13 @@ inline std::ostream & operator <<(std::ostream & s, const PIMathMatrixT inline PICout operator<<(PICout s, const PIMathMatrixT &m) { s << "{"; - PIMM_FOR_I(r, c) s << m[r][c]; - if (c < Cols - 1 || r < Rows - 1) s << ", "; } - if (r < Rows - 1) s << PICoutManipulators::NewLine << " "; } + for (uint r = 0; r < Rows; ++r) { + for (uint c = 0; c < Cols; ++c) { + s << m[r][c]; + if (c < Cols - 1 || r < Rows - 1) s << ", "; + } + if (r < Rows - 1) s << PICoutManipulators::NewLine << " "; + } s << "}"; return s; } @@ -804,9 +678,6 @@ template class PIMathMatrix; #undef PIMM_FOR -#undef PIMM_FOR_WB -#undef PIMM_FOR_I -#undef PIMM_FOR_I_WB #undef PIMM_FOR_C #undef PIMM_FOR_R diff --git a/libs/main/math/piquaternion.cpp b/libs/main/math/piquaternion.cpp index 9dc570a2..d71c5389 100644 --- a/libs/main/math/piquaternion.cpp +++ b/libs/main/math/piquaternion.cpp @@ -96,16 +96,6 @@ void PIQuaternion::normalize() { } -PIMathMatrixT44d PIQuaternion::makeMatrix() const { - PIMathMatrixT44d ret; - ret[0][0] = q[0]; ret[0][1] = -q[1]; ret[0][2] = -q[2]; ret[0][3] = -q[3]; - ret[1][0] = q[1]; ret[1][1] = q[0]; ret[1][2] = -q[3]; ret[1][3] = q[2]; - ret[2][0] = q[2]; ret[2][1] = q[3]; ret[2][2] = q[0]; ret[2][3] = -q[1]; - ret[3][0] = q[3]; ret[3][1] = -q[2]; ret[3][2] = q[1]; ret[3][3] = q[0]; - return ret; -} - - PIQuaternion PIQuaternion::fromEyler(double ax, double ay, double az) { PIQuaternion q_heading; PIQuaternion q_pinch; diff --git a/libs/main/math/piquaternion.h b/libs/main/math/piquaternion.h index c54dbf89..4cfb6f1f 100644 --- a/libs/main/math/piquaternion.h +++ b/libs/main/math/piquaternion.h @@ -52,9 +52,6 @@ public: protected: double q[4]; - PIMathMatrixT44d makeMatrix() const; - - }; PIP_EXPORT PIQuaternion operator *(const double & a, const PIQuaternion & q);