584 lines
23 KiB
C++
584 lines
23 KiB
C++
/*! \file pimathmatrix.h
|
|
* \brief PIMathMatrix
|
|
*/
|
|
/*
|
|
PIP - Platform Independent Primitives
|
|
PIMathMatrix
|
|
Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU Lesser 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 Lesser General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Lesser General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#ifndef PIMATHMATRIX_H
|
|
#define PIMATHMATRIX_H
|
|
|
|
#include "pimathvector.h"
|
|
#include "pimathcomplex.h"
|
|
|
|
|
|
|
|
template<typename T>
|
|
inline bool _PIMathMatrixNullCompare(const T v) {
|
|
static_assert(std::is_floating_point<T>::value, "Type must be floating point");
|
|
return (piAbs(v) < T(1E-200));
|
|
}
|
|
|
|
template<>
|
|
inline bool _PIMathMatrixNullCompare<complexf >(const complexf v) {
|
|
return (abs(v) < float(1E-200));
|
|
}
|
|
template<>
|
|
inline bool _PIMathMatrixNullCompare<complexd >(const complexd v) {
|
|
return (abs(v) < double(1E-200));
|
|
}
|
|
|
|
|
|
/// 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)
|
|
|
|
#pragma pack(push, 1)
|
|
template<uint Rows, uint Cols = Rows, typename Type = double>
|
|
class PIP_EXPORT PIMathMatrixT {
|
|
typedef PIMathMatrixT<Rows, Cols, Type> _CMatrix;
|
|
typedef PIMathMatrixT<Cols, Rows, Type> _CMatrixI;
|
|
typedef PIMathVectorT<Rows, Type> _CMCol;
|
|
typedef PIMathVectorT<Cols, Type> _CMRow;
|
|
static_assert(std::is_arithmetic<Type>::value, "Type must be arithmetic");
|
|
static_assert(Rows > 0, "Row count must be > 0");
|
|
static_assert(Cols > 0, "Column count must be > 0");
|
|
public:
|
|
PIMathMatrixT() {resize(Rows, Cols);}
|
|
PIMathMatrixT(const PIVector<Type> & val) {resize(Rows, Cols); int i = 0; PIMM_FOR_I_WB(r, c) m[r][c] = val[i++];}
|
|
|
|
static _CMatrix identity() {_CMatrix tm = _CMatrix(); PIMM_FOR_WB(r, c) tm.m[r][c] = (c == r ? Type(1) : Type(0)); return tm;}
|
|
static _CMatrix filled(const Type & v) {_CMatrix tm; PIMM_FOR_WB(r, c) tm.m[r][c] = v; return tm;}
|
|
static _CMatrix rotation(double angle) {return _CMatrix();}
|
|
static _CMatrix rotationX(double angle) {return _CMatrix();}
|
|
static _CMatrix rotationY(double angle) {return _CMatrix();}
|
|
static _CMatrix rotationZ(double angle) {return _CMatrix();}
|
|
static _CMatrix scaleX(double factor) {return _CMatrix();}
|
|
static _CMatrix scaleY(double factor) {return _CMatrix();}
|
|
static _CMatrix scaleZ(double factor) {return _CMatrix();}
|
|
|
|
uint cols() const {return Cols;}
|
|
uint rows() const {return Rows;}
|
|
_CMCol col(uint index) {_CMCol tv; PIMM_FOR_R(i) tv[i] = m[i][index]; return tv;}
|
|
_CMRow row(uint index) {_CMRow tv; PIMM_FOR_C(i) tv[i] = m[index][i]; return tv;}
|
|
_CMatrix & setCol(uint index, const _CMCol & v) {PIMM_FOR_R(i) m[i][index] = v[i]; return *this;}
|
|
_CMatrix & setRow(uint index, const _CMRow & v) {PIMM_FOR_C(i) m[index][i] = v[i]; return *this;}
|
|
_CMatrix & swapRows(uint r0, uint r1) {Type t; PIMM_FOR_C(i) {t = m[r0][i]; m[r0][i] = m[r1][i]; m[r1][i] = t;} return *this;}
|
|
_CMatrix & swapCols(uint c0, uint c1) {Type t; PIMM_FOR_R(i) {t = m[i][c0]; m[i][c0] = m[i][c1]; m[i][c1] = t;} return *this;}
|
|
_CMatrix & fill(const Type & v) {PIMM_FOR_WB(r, c) m[r][c] = v; return *this;}
|
|
bool isSquare() const {return cols() == rows();}
|
|
bool isIdentity() const {PIMM_FOR_WB(r, c) if ((c == r) ? m[r][c] != Type(1) : m[r][c] != Type(0)) return false; return true;}
|
|
bool isNull() const {PIMM_FOR_WB(r, c) if (m[r][c] != Type(0)) return false; return true;}
|
|
|
|
Type & at(uint row, uint col) {return m[row][col];}
|
|
Type at(uint row, uint col) const {return m[row][col];}
|
|
Type * operator [](uint row) {return m[row];}
|
|
const Type * operator [](uint row) const {return m[row];}
|
|
_CMatrix & operator =(const _CMatrix & sm) {memcpy(m, sm.m, sizeof(Type) * Cols * Rows); return *this;}
|
|
bool operator ==(const _CMatrix & sm) const {PIMM_FOR_WB(r, c) if (m[r][c] != sm.m[r][c]) return false; return true;}
|
|
bool operator !=(const _CMatrix & sm) const {return !(*this == sm);}
|
|
void operator +=(const _CMatrix & sm) {PIMM_FOR_WB(r, c) m[r][c] += sm.m[r][c];}
|
|
void operator -=(const _CMatrix & sm) {PIMM_FOR_WB(r, c) m[r][c] -= sm.m[r][c];}
|
|
void operator *=(const Type & v) {PIMM_FOR_WB(r, c) m[r][c] *= v;}
|
|
void operator /=(const Type & v) {PIMM_FOR_WB(r, c) m[r][c] /= v;}
|
|
_CMatrix operator -() const {_CMatrix tm; PIMM_FOR_WB(r, c) tm.m[r][c] = -m[r][c]; return tm;}
|
|
_CMatrix operator +(const _CMatrix & sm) const {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(r, c) tm.m[r][c] += sm.m[r][c]; return tm;}
|
|
_CMatrix operator -(const _CMatrix & sm) const {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(r, c) tm.m[r][c] -= sm.m[r][c]; return tm;}
|
|
_CMatrix operator *(const Type & v) const {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(r, c) tm.m[r][c] *= v; return tm;}
|
|
_CMatrix operator /(const Type & v) const {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(r, c) tm.m[r][c] /= v; return tm;}
|
|
|
|
Type determinant(bool * ok = 0) const {
|
|
_CMatrix m(*this);
|
|
bool k;
|
|
Type ret = Type(0);
|
|
m.toUpperTriangular(&k);
|
|
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];
|
|
return ret;
|
|
}
|
|
|
|
_CMatrix & toUpperTriangular(bool * ok = 0) {
|
|
if (Cols != Rows) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
_CMatrix smat(*this);
|
|
bool ndet;
|
|
uint crow;
|
|
Type mul;
|
|
for (uint i = 0; i < Cols; ++i) {
|
|
ndet = true;
|
|
for (uint j = 0; j < Rows; ++j) if (smat.m[i][j] != 0) ndet = false;
|
|
if (ndet) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
for (uint i = 0; i < Cols; ++i) {
|
|
crow = i;
|
|
while (smat.m[i][i] == Type(0))
|
|
smat.swapRows(i, ++crow);
|
|
for (uint j = i + 1; j < Rows; ++j) {
|
|
mul = smat.m[i][j] / smat.m[i][i];
|
|
for (uint k = i; k < Cols; ++k) smat.m[k][j] -= mul * smat.m[k][i];
|
|
}
|
|
if (i < Cols - 1) {
|
|
if (fabs(smat.m[i+1][i+1]) < Type(1E-200)) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
}
|
|
if (ok != 0) *ok = true;
|
|
memcpy(m, smat.m, sizeof(Type) * Cols * Rows);
|
|
return *this;
|
|
}
|
|
|
|
_CMatrix & invert(bool * ok = 0) {
|
|
static_assert(Cols == Rows, "Only square matrix invertable");
|
|
_CMatrix mtmp = _CMatrix::identity(), smat(*this);
|
|
bool ndet;
|
|
uint crow;
|
|
Type mul, iddiv;
|
|
for (uint i = 0; i < Cols; ++i) {
|
|
ndet = true;
|
|
for (uint j = 0; j < Rows; ++j) if (smat.m[i][j] != 0) ndet = false;
|
|
if (ndet) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
for (uint i = 0; i < Cols; ++i) {
|
|
crow = i;
|
|
while (smat.m[i][i] == Type(0)) {
|
|
++crow;
|
|
smat.swapRows(i, crow);
|
|
mtmp.swapRows(i, crow);
|
|
}
|
|
for (uint j = i + 1; j < Rows; ++j) {
|
|
mul = smat.m[i][j] / smat.m[i][i];
|
|
for (uint k = i; k < Cols; ++k) smat.m[k][j] -= mul * smat.m[k][i];
|
|
for (uint k = 0; k < Cols; ++k) mtmp.m[k][j] -= mul * mtmp.m[k][i];
|
|
}
|
|
if (i < Cols - 1) {
|
|
if (fabs(smat.m[i+1][i+1]) < Type(1E-200)) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
iddiv = smat.m[i][i];
|
|
for (uint j = i; j < Cols; ++j) smat.m[j][i] /= iddiv;
|
|
for (uint j = 0; j < Cols; ++j) mtmp.m[j][i] /= iddiv;
|
|
}
|
|
for (uint i = Cols - 1; i > 0; --i) {
|
|
for (uint j = 0; j < i; ++j) {
|
|
mul = smat.m[i][j];
|
|
smat.m[i][j] -= mul;
|
|
for (uint k = 0; k < Cols; ++k) mtmp.m[k][j] -= mtmp.m[k][i] * mul;
|
|
}
|
|
}
|
|
if (ok != 0) *ok = true;
|
|
memcpy(m, mtmp.m, sizeof(Type) * Cols * Rows);
|
|
return *this;
|
|
}
|
|
_CMatrix inverted(bool * ok = 0) const {_CMatrix tm(*this); tm.invert(ok); return tm;}
|
|
_CMatrixI transposed() const {_CMatrixI tm; PIMM_FOR_WB(r, c) tm[c][r] = m[r][c]; return tm;}
|
|
|
|
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;}
|
|
int c_, r_;
|
|
Type m[Rows][Cols];
|
|
|
|
};
|
|
#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<uint Rows, uint Cols, typename Type>
|
|
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrixT<Rows, Cols, Type> & 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;}
|
|
#endif
|
|
|
|
template<uint Rows, uint Cols, typename Type>
|
|
inline PICout operator <<(PICout s, const PIMathMatrixT<Rows, Cols, Type> & 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 << " ";} s << "}"; return s;}
|
|
|
|
/// Multiply matrices {Rows0 x CR} on {CR x Cols1}, result is {Rows0 x Cols1}
|
|
template<uint CR, uint Rows0, uint Cols1, typename Type>
|
|
inline PIMathMatrixT<Rows0, Cols1, Type> operator *(const PIMathMatrixT<Rows0, CR, Type> & fm,
|
|
const PIMathMatrixT<CR, Cols1, Type> & sm) {
|
|
PIMathMatrixT<Rows0, Cols1, Type> tm;
|
|
Type t;
|
|
for (uint j = 0; j < Rows0; ++j) {
|
|
for (uint i = 0; i < Cols1; ++i) {
|
|
t = Type(0);
|
|
for (uint k = 0; k < CR; ++k)
|
|
t += fm[j][k] * sm[k][i];
|
|
tm[j][i] = t;
|
|
}
|
|
}
|
|
return tm;
|
|
}
|
|
|
|
/// Multiply matrix {Rows x Cols} on vector {Cols}, result is vector {Rows}
|
|
template<uint Cols, uint Rows, typename Type>
|
|
inline PIMathVectorT<Rows, Type> operator *(const PIMathMatrixT<Rows, Cols, Type> & fm,
|
|
const PIMathVectorT<Cols, Type> & sv) {
|
|
PIMathVectorT<Rows, Type> tv;
|
|
Type t;
|
|
for (uint j = 0; j < Rows; ++j) {
|
|
t = Type(0);
|
|
for (uint i = 0; i < Cols; ++i)
|
|
t += fm[j][i] * sv[i];
|
|
tv[j] = t;
|
|
}
|
|
return tv;
|
|
}
|
|
|
|
/// Multiply vector {Rows} on matrix {Rows x Cols}, result is vector {Cols}
|
|
template<uint Cols, uint Rows, typename Type>
|
|
inline PIMathVectorT<Cols, Type> operator *(const PIMathVectorT<Rows, Type> & sv,
|
|
const PIMathMatrixT<Rows, Cols, Type> & fm) {
|
|
PIMathVectorT<Cols, Type> tv;
|
|
Type t;
|
|
for (uint j = 0; j < Cols; ++j) {
|
|
t = Type(0);
|
|
for (uint i = 0; i < Rows; ++i)
|
|
t += fm[i][j] * sv[i];
|
|
tv[j] = t;
|
|
}
|
|
return tv;
|
|
}
|
|
|
|
/// Multiply value(T) on matrix {Rows x Cols}, result is vector {Rows}
|
|
template<uint Cols, uint Rows, typename Type>
|
|
inline PIMathMatrixT<Rows, Cols, Type> operator *(const Type & x, const PIMathMatrixT<Rows, Cols, Type> & v) {
|
|
return v * x;
|
|
}
|
|
|
|
|
|
typedef PIMathMatrixT<2u, 2u, int> PIMathMatrixT22i;
|
|
typedef PIMathMatrixT<3u, 3u, int> PIMathMatrixT33i;
|
|
typedef PIMathMatrixT<4u, 4u, int> PIMathMatrixT44i;
|
|
typedef PIMathMatrixT<2u, 2u, double> PIMathMatrixT22d;
|
|
typedef PIMathMatrixT<3u, 3u, double> PIMathMatrixT33d;
|
|
typedef PIMathMatrixT<4u, 4u, double> PIMathMatrixT44d;
|
|
|
|
|
|
template<typename Type>
|
|
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
|
|
|
|
|
|
|
|
|
|
|
|
/// Matrix
|
|
|
|
#define PIMM_FOR(c, r) for (uint c = 0; c < _V2D::cols_; ++c) for (uint r = 0; r < _V2D::rows_; ++r)
|
|
#define PIMM_FOR_I(c, r) for (uint r = 0; r < _V2D::rows_; ++r) for (uint c = 0; c < _V2D::cols_; ++c)
|
|
#define PIMM_FOR_A(v) for (uint v = 0; v < _V2D::mat.size(); ++v)
|
|
#define PIMM_FOR_C(v) for (uint v = 0; v < _V2D::cols_; ++v)
|
|
#define PIMM_FOR_R(v) for (uint v = 0; v < _V2D::rows_; ++v)
|
|
|
|
template<typename Type>
|
|
class PIP_EXPORT PIMathMatrix : public PIVector2D<Type> {
|
|
typedef PIVector2D<Type> _V2D;
|
|
typedef PIMathMatrix<Type> _CMatrix;
|
|
typedef PIMathVector<Type> _CMCol;
|
|
public:
|
|
PIMathMatrix(const uint cols = 0, const uint rows = 0, const Type & f = Type()) {_V2D::resize(rows, cols, f);}
|
|
PIMathMatrix(const uint cols, const uint rows, const PIVector<Type> & val) {_V2D::resize(rows, cols); int i=0; PIMM_FOR_I(c, r) _V2D::element(r, c) = val[i++];}
|
|
PIMathMatrix(const PIVector<PIVector<Type> > & val) {if(!val.isEmpty()) {_V2D::resize(val.size(), val[0].size()); PIMM_FOR_I(c, r) _V2D::element(r, c) = val[r][c];}}
|
|
PIMathMatrix(const PIVector2D<Type> & val) {if(!val.isEmpty()) {_V2D::resize(val.rows(), val.cols()); PIMM_FOR_I(c, r) _V2D::element(r, c) = val.element(r, c);}}
|
|
|
|
static _CMatrix identity(const uint cols, const uint rows) {_CMatrix tm(cols, rows); for (uint r = 0; r < rows; ++r) for (uint c = 0; c < cols; ++c) tm.element(r, c) = (c == r ? Type(1) : Type(0)); return tm;}
|
|
static _CMatrix matrixRow(const PIMathVector<Type> & val) {return _CMatrix(val.size(), 1, val.toVector());}
|
|
static _CMatrix matrixCol(const PIMathVector<Type> & val) {return _CMatrix(1, val.size(), val.toVector());}
|
|
|
|
_CMatrix & setCol(uint index, const _CMCol & v) {PIMM_FOR_R(i) _V2D::element(i, index) = v[i]; return *this;}
|
|
_CMatrix & setRow(uint index, const _CMCol & v) {PIMM_FOR_C(i) _V2D::element(index, i) = v[i]; return *this;}
|
|
_CMatrix & swapCols(uint r0, uint r1) {PIMM_FOR_C(i) {piSwap(_V2D::element(i, r0), _V2D::element(i, r1));} return *this;}
|
|
_CMatrix & swapRows(uint c0, uint c1) {PIMM_FOR_R(i) {piSwap(_V2D::element(c0, i), _V2D::element(c1, i));} return *this;}
|
|
_CMatrix & fill(const Type & v) {PIMM_FOR_A(i) _V2D::mat[i] = v; return *this;}
|
|
bool isSquare() const {return _V2D::cols_ == _V2D::rows_;}
|
|
bool isIdentity() const {PIMM_FOR(c, r) if ((c == r) ? _V2D::element(r, c) != Type(1) : _V2D::element(r, c) != Type(0)) return false; return true;}
|
|
bool isNull() const {PIMM_FOR_A(i) if (_V2D::mat[i] != Type(0)) return false; return true;}
|
|
bool isValid() const {return !PIVector2D<Type>::isEmpty();}
|
|
|
|
_CMatrix & operator =(const PIVector<PIVector<Type> > & v) {*this = _CMatrix(v); return *this;}
|
|
bool operator ==(const _CMatrix & sm) const {
|
|
if(_V2D::mat.size() != sm.mat.size())
|
|
return false;
|
|
PIMM_FOR_A(i) {
|
|
if (_V2D::mat[i] != sm.mat[i])
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
bool operator !=(const _CMatrix & sm) const {return !(*this == sm);}
|
|
void operator +=(const _CMatrix & sm) {PIMM_FOR_A(i) _V2D::mat[i] += sm.mat[i];}
|
|
void operator -=(const _CMatrix & sm) {PIMM_FOR_A(i) _V2D::mat[i] -= sm.mat[i];}
|
|
void operator *=(const Type & v) {PIMM_FOR_A(i) _V2D::mat[i] *= v;}
|
|
void operator /=(const Type & v) {PIMM_FOR_A(i) _V2D::mat[i] /= v;}
|
|
_CMatrix operator -() const {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] = -_V2D::mat[i]; return tm;}
|
|
_CMatrix operator +(const _CMatrix & sm) const {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] += sm.mat[i]; return tm;}
|
|
_CMatrix operator -(const _CMatrix & sm) const {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] -= sm.mat[i]; return tm;}
|
|
_CMatrix operator *(const Type & v) const {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] *= v; return tm;}
|
|
_CMatrix operator /(const Type & v) const {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] /= v; return tm;}
|
|
|
|
Type determinant(bool * ok = 0) const {
|
|
_CMatrix m(*this);
|
|
bool k;
|
|
Type ret = Type(0);
|
|
m.toUpperTriangular(&k);
|
|
if (ok) *ok = k;
|
|
if (!k) return ret;
|
|
ret = Type(1);
|
|
for (uint c = 0; c < _V2D::cols_; ++c)
|
|
for (uint r = 0; r < _V2D::rows_; ++r)
|
|
if (r == c)
|
|
ret *= m.element(r, c);
|
|
return ret;
|
|
}
|
|
|
|
Type trace(bool * ok = 0) const {
|
|
Type ret = Type(0);
|
|
if (!isSquare()) {
|
|
if (ok != 0) *ok = false;
|
|
return ret;
|
|
}
|
|
for (uint i = 0; i < _V2D::cols_; ++i) {
|
|
ret += _V2D::element(i, i);
|
|
}
|
|
if (ok != 0) *ok = true;
|
|
return ret;
|
|
}
|
|
|
|
_CMatrix & toUpperTriangular(bool * ok = 0) {
|
|
if (!isSquare()) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
_CMatrix smat(*this);
|
|
bool ndet;
|
|
uint crow;
|
|
Type mul;
|
|
for (uint i = 0; i < _V2D::cols_; ++i) {
|
|
ndet = true;
|
|
for (uint j = 0; j < _V2D::rows_; ++j) if (smat.element(i, j) != 0) ndet = false;
|
|
if (ndet) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
for (uint i = 0; i < _V2D::cols_; ++i) {
|
|
crow = i;
|
|
while (smat.element(i, i) == Type(0))
|
|
smat.swapRows(i, ++crow);
|
|
for (uint j = i + 1; j < _V2D::rows_; ++j) {
|
|
mul = smat.element(i, j) / smat.element(i, i);
|
|
for (uint k = i; k < _V2D::cols_; ++k) smat.element(k, j) -= mul * smat.element(k, i);
|
|
}
|
|
if (i < _V2D::cols_ - 1) {
|
|
if (_PIMathMatrixNullCompare(smat.element(i+1, i+1))) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
}
|
|
if (ok != 0) *ok = true;
|
|
_V2D::mat.swap(smat.mat);
|
|
return *this;
|
|
}
|
|
|
|
_CMatrix & invert(bool * ok = 0, _CMCol * sv = 0) {
|
|
if (!isSquare()) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
_CMatrix mtmp = _CMatrix::identity(_V2D::cols_, _V2D::rows_), smat(*this);
|
|
bool ndet;
|
|
uint crow;
|
|
Type mul, iddiv;
|
|
for (uint i = 0; i < _V2D::cols_; ++i) {
|
|
ndet = true;
|
|
for (uint j = 0; j < _V2D::rows_; ++j) if (smat.element(i, j) != Type(0)) ndet = false;
|
|
if (ndet) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
for (uint i = 0; i < _V2D::cols_; ++i) {
|
|
crow = i;
|
|
while (smat.element(i, i) == Type(0)) {
|
|
++crow;
|
|
smat.swapRows(i, crow);
|
|
mtmp.swapRows(i, crow);
|
|
if (sv != 0) sv->swap(i, crow);
|
|
}
|
|
for (uint j = i + 1; j < _V2D::rows_; ++j) {
|
|
mul = smat.element(i, j) / smat.element(i, i);
|
|
for (uint k = i; k < _V2D::cols_; ++k) smat.element(k, j) -= mul * smat.element(k, i);
|
|
for (uint k = 0; k < _V2D::cols_; ++k) mtmp.element(k, j) -= mul * mtmp.element(k, i);
|
|
if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
|
|
}
|
|
if (i < _V2D::cols_ - 1) {
|
|
if (_PIMathMatrixNullCompare(smat.element(i+1, i+1))) {
|
|
if (ok != 0) *ok = false;
|
|
return *this;
|
|
}
|
|
}
|
|
iddiv = smat.element(i, i);
|
|
for (uint j = i; j < _V2D::cols_; ++j) smat.element(j, i) /= iddiv;
|
|
for (uint j = 0; j < _V2D::cols_; ++j) mtmp.element(j, i) /= iddiv;
|
|
if (sv != 0) (*sv)[i] /= iddiv;
|
|
}
|
|
for (uint i = _V2D::cols_ - 1; i > 0; --i) {
|
|
for (uint j = 0; j < i; ++j) {
|
|
mul = smat.element(i, j);
|
|
smat.element(i, j) -= mul;
|
|
for (uint k = 0; k < _V2D::cols_; ++k) mtmp.element(k, j) -= mul * mtmp.element(k, i);
|
|
if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
|
|
}
|
|
}
|
|
if (ok != 0) *ok = true;
|
|
PIVector2D<Type>::swap(mtmp);
|
|
return *this;
|
|
}
|
|
_CMatrix inverted(bool * ok = 0) const {_CMatrix tm(*this); tm.invert(ok); return tm;}
|
|
_CMatrix transposed() const {_CMatrix tm(_V2D::rows_, _V2D::cols_); PIMM_FOR(c, r) tm.element(c, r) = _V2D::element(r, c); return tm;}
|
|
};
|
|
|
|
|
|
#ifdef PIP_STD_IOSTREAM
|
|
template<typename Type>
|
|
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrix<Type> & m) {s << "{"; for (uint r = 0; r < m.rows(); ++r) { for (uint c = 0; c < m.cols(); ++c) { s << m.element(r, c); if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";} if (r < m.rows() - 1) s << std::endl << " ";} s << "}"; return s;}
|
|
#endif
|
|
|
|
template<typename Type>
|
|
inline PICout operator <<(PICout s, const PIMathMatrix<Type> & m) {s << "Matrix{"; for (uint r = 0; r < m.rows(); ++r) { for (uint c = 0; c < m.cols(); ++c) { s << m.element(r, c); if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";} if (r < m.rows() - 1) s << PICoutManipulators::NewLine << " ";} s << "}"; return s;}
|
|
|
|
template<typename Type>
|
|
inline PIByteArray & operator <<(PIByteArray & s, const PIMathMatrix<Type> & v) {s << (const PIVector2D<Type> &)v; return s;}
|
|
template<typename Type>
|
|
inline PIByteArray & operator >>(PIByteArray & s, PIMathMatrix<Type> & v) {s >> (PIVector2D<Type> &)v; return s;}
|
|
|
|
|
|
/// Multiply matrices {CR x Rows0} on {Cols1 x CR}, result is {Cols1 x Rows0}
|
|
template<typename Type>
|
|
inline PIMathMatrix<Type> operator *(const PIMathMatrix<Type> & fm,
|
|
const PIMathMatrix<Type> & sm) {
|
|
uint cr = fm.cols(), rows0 = fm.rows(), cols1 = sm.cols();
|
|
PIMathMatrix<Type> tm(cols1, rows0);
|
|
if (fm.cols() != sm.rows()) return tm;
|
|
Type t;
|
|
for (uint j = 0; j < rows0; ++j) {
|
|
for (uint i = 0; i < cols1; ++i) {
|
|
t = Type(0);
|
|
for (uint k = 0; k < cr; ++k)
|
|
t += fm.element(j, k) * sm.element(k, i);
|
|
tm.element(j, i) = t;
|
|
}
|
|
}
|
|
return tm;
|
|
}
|
|
|
|
/// Multiply matrix {Cols x Rows} on vector {Cols}, result is vector {Rows}
|
|
template<typename Type>
|
|
inline PIMathVector<Type> operator *(const PIMathMatrix<Type> & fm,
|
|
const PIMathVector<Type> & sv) {
|
|
uint c = fm.cols(), r = fm.rows();
|
|
PIMathVector<Type> tv(r);
|
|
if (c != sv.size()) return tv;
|
|
Type t;
|
|
for (uint j = 0; j < r; ++j) {
|
|
t = Type(0);
|
|
for (uint i = 0; i < c; ++i)
|
|
t += fm.element(j, i) * sv[i];
|
|
tv[j] = t;
|
|
}
|
|
return tv;
|
|
}
|
|
|
|
|
|
/// Multiply vector {Rows} on matrix {Rows x Cols}, result is vector {Cols}
|
|
template<typename Type>
|
|
inline PIMathVector<Type> operator *(const PIMathVector<Type> & sv,
|
|
const PIMathMatrix<Type> & fm) {
|
|
uint c = fm.cols(), r = fm.rows();
|
|
PIMathVector<Type> tv(c);
|
|
Type t;
|
|
for (uint j = 0; j < c; ++j) {
|
|
t = Type(0);
|
|
for (uint i = 0; i < r; ++i)
|
|
t += fm.element(i, j) * sv[i];
|
|
tv[j] = t;
|
|
}
|
|
return tv;
|
|
}
|
|
|
|
/// Multiply value(T) on matrix {Rows x Cols}, result is vector {Rows}
|
|
template<typename Type>
|
|
inline PIMathMatrix<Type> operator *(const Type & x, const PIMathMatrix<Type> & v) {
|
|
return v * x;
|
|
}
|
|
|
|
typedef PIMathMatrix<int> PIMathMatrixi;
|
|
typedef PIMathMatrix<double> PIMathMatrixd;
|
|
|
|
template<typename T>
|
|
PIMathMatrix<complex<T> > hermitian(const PIMathMatrix<complex<T> > & m) {
|
|
PIMathMatrix<complex<T> > ret(m);
|
|
for (uint r = 0; r < ret.rows(); ++r) for (uint c = 0; c < ret.cols(); ++c) ret.element(r, c).imag(-(ret.element(r, c).imag()));
|
|
return ret.transposed();
|
|
}
|
|
|
|
#undef PIMM_FOR
|
|
#undef PIMM_FOR_I
|
|
#undef PIMM_FOR_A
|
|
#undef PIMM_FOR_C
|
|
#undef PIMM_FOR_R
|
|
|
|
#endif // PIMATHMATRIX_H
|