SHS/pip
4
3
Fork 0
Code Issues 67 Pull Requests 1 Actions Releases Wiki Activity

git-svn-id: svn://db.shs.com.ru/pip@656 12ceb7fc-bf1f-11e4-8940-5bc7170c53b5

Browse Source
This commit is contained in:
Бычков Андрей
2018-11-12 15:26:34 +00:00
parent 1f0810ab1e
commit e35313dc1a
5 changed files with 166 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

212
src_main/math/pimathmatrix.h
View File

@@ -24,6 +24,24 @@
#define PIMATHMATRIX_H
#include "pimathvector.h"
#include "pimathcomplex.h"
template<typename T>
inline bool _PIMathMatrixNullCompare(const T v) {
return (piAbs(v) < T(1E-100));
}
template<>
inline bool _PIMathMatrixNullCompare<complexf >(const complexf v) {
return (abs(v) < float(1E-100));
}
template<>
inline bool _PIMathMatrixNullCompare<complexd >(const complexd v) {
return (abs(v) < double(1E-100));
}
/// Matrix templated
@@ -43,7 +61,6 @@ class PIP_EXPORT PIMathMatrixT {
typedef PIMathVectorT<Cols, Type> _CMRow;
public:
PIMathMatrixT() {resize(Rows, Cols);}
PIMathMatrixT(Type fval, ...) {resize(Rows, Cols); va_list vl; va_start(vl, fval); PIMM_FOR_I_WB(r, c) m[r][c] = (r + c == 0 ? fval : va_arg(vl, Type)); va_end(vl);}
PIMathMatrixT(const PIVector<Type> & val) {resize(Rows, Cols); int i = 0; PIMM_FOR_I_WB(r, c) m[r][c] = val[i++];}
//PIMathMatrixT(const _CMatrix & o) {resize(Rows, Cols); int i = 0; PIMM_FOR_I_WB(r, c) m[r][c] = val[i++];}
@@ -66,8 +83,6 @@ public:
_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;}
//inline _CMatrix & set(Type fval, ...) {m[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) m[i] = va_arg(vl, Type); va_end(vl); return *this;}
//inline void normalize() {Type tv = length(); if (tv == Type(1)) return; PIMV_FOR(i, 0) m[i] /= tv;}
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;}
@@ -298,7 +313,6 @@ typedef PIMathMatrixT<4u, 4u, double> PIMathMatrixT44d;
template<typename Type>
class PIMathMatrix;
#undef PIMV_FOR
#undef PIMM_FOR
#undef PIMM_FOR_WB
#undef PIMM_FOR_I
@@ -312,60 +326,79 @@ class PIMathMatrix;
/// Matrix
#define PIMM_FOR(c, r) for (uint c = 0; c < cols_; ++c) { for (uint r = 0; r < rows_; ++r) {
#define PIMM_FOR_WB(c, r) for (uint c = 0; c < cols_; ++c) for (uint r = 0; r < rows_; ++r) // without brakes
#define PIMM_FOR_I(c, r) for (uint r = 0; r < rows_; ++r) { for (uint c = 0; c < cols_; ++c) {
#define PIMM_FOR_I_WB(c, r) 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(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 {
class PIP_EXPORT PIMathMatrix : public PIVector2D<Type> {
typedef PIVector2D<Type> _V2D;
typedef PIMathMatrix<Type> _CMatrix;
typedef PIMathVector<Type> _CMCol;
typedef PIMathVector<Type> _CMRow;
public:
PIMathMatrix(const uint cols = 3, const uint rows = 3) {resize(cols, rows);}
PIMathMatrix(const uint cols, const uint rows, Type fval, ...) {resize(cols, rows); va_list vl; va_start(vl, fval); PIMM_FOR_I_WB(c, r) m[c][r] = (r + c == 0 ? fval : va_arg(vl, Type)); va_end(vl);}
PIMathMatrix(const uint cols, const uint rows, const PIVector<Type> & val) {resize(cols, rows); int i = 0; PIMM_FOR_I_WB(c, r) m[c][r] = val[i++];}
PIMathMatrix(const uint cols, const uint rows, const PIVector<Type> & val) {resize(cols, rows); int i=0; PIMM_FOR_I(c, r) (*this)[r][c] = val[i++];}
PIMathMatrix(const PIVector<PIVector<Type> > & val) {_V2D::cols_ = _V2D::rows_ = 0; if(!val.isEmpty()) {resize(val[0].size(), val.size()); PIMM_FOR_I(c, r) (*this)[r][c] = val[r][c];}}
PIMathMatrix(const PIVector2D<Type> & val) {_V2D::cols_ = _V2D::rows_ = 0; if(!val.isEmpty()) {resize(val.cols(), val.rows()); PIMM_FOR_I(c, r) (*this)[r][c] = val[r][c];}}
static _CMatrix identity(const uint cols_, const uint rows_) {_CMatrix tm(cols_, rows_); PIMM_FOR_WB(c, r) tm.m[c][r] = (c == r ? Type(1) : Type(0)); return tm;}
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[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());}
uint cols() const {return cols_;}
uint rows() const {return rows_;}
_CMCol col(uint index) {_CMCol tv; PIMM_FOR_R(i) tv[i] = m[index][i]; return tv;}
_CMRow row(uint index) {_CMRow tv; PIMM_FOR_C(i) tv[i] = m[i][index]; return tv;}
_CMatrix & resize(const uint cols, const uint rows, const Type & new_value = Type()) {cols_ = cols; rows_ = rows; m.resize(cols); PIMM_FOR_C(i) m[i].resize(rows, new_value); return *this;}
_CMatrix & setCol(uint index, const _CMCol & v) {PIMM_FOR_R(i) m[index][i] = v[i]; return *this;}
_CMatrix & setRow(uint index, const _CMRow & v) {PIMM_FOR_C(i) m[i][index] = v[i]; return *this;}
_CMatrix & swapRows(uint r0, uint r1) {Type t; PIMM_FOR_C(i) {t = m[i][r0]; m[i][r0] = m[i][r1]; m[i][r1] = t;} return *this;}
_CMatrix & swapCols(uint c0, uint c1) {Type t; PIMM_FOR_R(i) {t = m[c0][i]; m[c0][i] = m[c1][i]; m[c1][i] = t;} return *this;}
_CMatrix & fill(const Type & v) {PIMM_FOR_WB(c, r) m[c][r] = v; return *this;}
//inline _CMatrix & set(Type fval, ...) {m[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) m[i] = va_arg(vl, Type); va_end(vl); return *this;}
//inline void normalize() {Type tv = length(); if (tv == Type(1)) return; PIMV_FOR(i, 0) m[i] /= tv;}
bool isSquare() const {return cols() == rows();}
bool isIdentity() const {PIMM_FOR_WB(c, r) if ((c == r) ? m[c][r] != Type(1) : m[c][r] != Type(0)) return false; return true;}
bool isNull() const {PIMM_FOR_WB(c, r) if (m[c][r] != Type(0)) return false; return true;}
_CMatrix & resize(const uint cols, const uint rows, const Type & new_value = Type()) {_V2D::_resizeRaw(rows, cols); PIMM_FOR_A(i) _V2D::mat[i] = new_value; return *this;}
_CMatrix & swapCols(uint r0, uint r1) {Type t; PIMM_FOR_C(i) {t = (*this)[i][r0]; (*this)[i][r0] = (*this)[i][r1]; (*this)[i][r1] = t;} return *this;}
_CMatrix & swapRows(uint c0, uint c1) {Type t; PIMM_FOR_R(i) {t = (*this)[c0][i]; (*this)[c0][i] = (*this)[c1][i]; (*this)[c1][i] = t;} 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) ? (*this)[c][r] != Type(1) : (*this)[c][r] != Type(0)) return false; return true;}
bool isNull() const {PIMM_FOR_A(i) if (_V2D::mat[i] != Type(0)) return false; return true;}
Type & at(uint col, uint row) {return m[col][row];}
Type at(uint col, uint row) const {return m[col][row];}
PIVector<Type> & operator [](uint col) {return m[col];}
PIVector<Type> operator [](uint col) const {return m[col];}
void operator =(const _CMatrix & sm) {m = sm.m;}
bool operator ==(const _CMatrix & sm) const {PIMM_FOR_WB(c, r) if (m[c][r] != sm.m[c][r]) return false; return true;}
_CMatrix & operator =(const PIVector<PIVector<Type> > & v) {*this = PIVector2D<Type>(v); return *this;}
bool operator ==(const _CMatrix & sm) const {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_WB(c, r) m[c][r] += sm.m[c][r];}
void operator -=(const _CMatrix & sm) {PIMM_FOR_WB(c, r) m[c][r] -= sm.m[c][r];}
void operator *=(const Type & v) {PIMM_FOR_WB(c, r) m[c][r] *= v;}
void operator /=(const Type & v) {PIMM_FOR_WB(c, r) m[c][r] /= v;}
_CMatrix operator -() {_CMatrix tm(*this); PIMM_FOR_WB(c, r) tm.m[c][r] = -m[c][r]; return tm;}
_CMatrix operator +(const _CMatrix & sm) {_CMatrix tm(*this); PIMM_FOR_WB(c, r) tm.m[c][r] += sm.m[c][r]; return tm;}
_CMatrix operator -(const _CMatrix & sm) {_CMatrix tm(*this); PIMM_FOR_WB(c, r) tm.m[c][r] -= sm.m[c][r]; return tm;}
_CMatrix operator *(const Type & v) {_CMatrix tm(*this); PIMM_FOR_WB(c, r) tm.m[c][r] *= v; return tm;}
_CMatrix operator /(const Type & v) {_CMatrix tm(*this); PIMM_FOR_WB(c, r) tm.m[c][r] /= v; return tm;}
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 -() {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] = -_V2D::mat[i]; return tm;}
_CMatrix operator +(const _CMatrix & sm) {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] += sm.mat[i]; return tm;}
_CMatrix operator -(const _CMatrix & sm) {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] -= sm.mat[i]; return tm;}
_CMatrix operator *(const Type & v) {_CMatrix tm(*this); PIMM_FOR_A(i) tm.mat[i] *= v; return tm;}
_CMatrix operator /(const Type & v) {_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[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 += (*this)[i][i];
}
if (ok != 0) *ok = true;
return ret;
}
_CMatrix & toUpperTriangular(bool * ok = 0) {
if (cols_ != rows_) {
if (!isSquare()) {
if (ok != 0) *ok = false;
return *this;
}
@@ -373,115 +406,115 @@ public:
bool ndet;
uint crow;
Type mul;
for (uint i = 0; i < cols_; ++i) {
for (uint i = 0; i < _V2D::cols_; ++i) {
ndet = true;
for (uint j = 0; j < rows_; ++j) if (smat.m[i][j] != 0) ndet = false;
for (uint j = 0; j < _V2D::rows_; ++j) if (smat.mat[i][j] != 0) ndet = false;
if (ndet) {
if (ok != 0) *ok = false;
return *this;
}
for (uint j = 0; j < cols_; ++j) if (smat.m[j][i] != 0) ndet = false;
for (uint j = 0; j < _V2D::cols_; ++j) if (smat.mat[j][i] != 0) ndet = false;
if (ndet) {
if (ok != 0) *ok = false;
return *this;
}
}
for (uint i = 0; i < cols_; ++i) {
for (uint i = 0; i < _V2D::cols_; ++i) {
crow = i;
while (smat.m[i][i] == Type(0))
while (smat.mat[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];
for (uint j = i + 1; j < _V2D::rows_; ++j) {
mul = smat.mat[i][j] / smat.mat[i][i];
for (uint k = i; k < _V2D::cols_; ++k) smat.mat[k][j] -= mul * smat.mat[k][i];
}
if (i < cols_ - 1) {
if (fabs(smat.m[i+1][i+1]) < Type(1E-100)) {
if (i < _V2D::cols_ - 1) {
if (fabs(smat.mat[i+1][i+1]) < Type(1E-100)) {
if (ok != 0) *ok = false;
return *this;
}
}
}
if (ok != 0) *ok = true;
m = smat.m;
_V2D::mat.swap(smat.mat);
return *this;
}
_CMatrix & invert(bool * ok = 0, _CMCol * sv = 0) {
if (cols_ != rows_) {
if (!isSquare()) {
if (ok != 0) *ok = false;
return *this;
}
_CMatrix mtmp = _CMatrix::identity(cols_, rows_), smat(*this);
_CMatrix mtmp = _CMatrix::identity(_V2D::cols_, _V2D::rows_), smat(*this);
bool ndet;
uint crow;
Type mul, iddiv;
for (uint i = 0; i < cols_; ++i) {
for (uint i = 0; i < _V2D::cols_; ++i) {
ndet = true;
for (uint j = 0; j < rows_; ++j) if (smat.m[i][j] != 0) ndet = false;
for (uint j = 0; j < _V2D::rows_; ++j) if (smat[i][j] != Type(0)) ndet = false;
if (ndet) {
if (ok != 0) *ok = false;
return *this;
}
for (uint j = 0; j < cols_; ++j) if (smat.m[j][i] != 0) ndet = false;
for (uint j = 0; j < _V2D::cols_; ++j) if (smat[j][i] != Type(0)) ndet = false;
if (ndet) {
if (ok != 0) *ok = false;
return *this;
}
}
for (uint i = 0; i < cols_; ++i) {
for (uint i = 0; i < _V2D::cols_; ++i) {
crow = i;
while (smat.m[i][i] == Type(0)) {
while (smat[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 < 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];
for (uint j = i + 1; j < _V2D::rows_; ++j) {
mul = smat[i][j] / smat[i][i];
for (uint k = i; k < _V2D::cols_; ++k) smat[k][j] -= mul * smat[k][i];
for (uint k = 0; k < _V2D::cols_; ++k) mtmp[k][j] -= mul * mtmp[k][i];
if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
}
//cout << i << endl << smat << endl;
if (i < cols_ - 1) {
if (fabs(smat.m[i+1][i+1]) < Type(1E-100)) {
if (i < _V2D::cols_ - 1) {
if (_PIMathMatrixNullCompare(smat[i+1][i+1])) {
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;
iddiv = smat[i][i];
for (uint j = i; j < _V2D::cols_; ++j) smat[j][i] /= iddiv;
for (uint j = 0; j < _V2D::cols_; ++j) mtmp[j][i] /= iddiv;
if (sv != 0) (*sv)[i] /= iddiv;
}
for (uint i = cols_ - 1; i > 0; --i) {
for (uint i = _V2D::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;
mul = smat[i][j];
smat[i][j] -= mul;
for (uint k = 0; k < _V2D::cols_; ++k) mtmp[k][j] -= mtmp[k][i] * mul;
if (sv != 0) (*sv)[j] -= mul * (*sv)[i];
}
}
if (ok != 0) *ok = true;
m = mtmp.m;
PIVector2D<Type>::swap(mtmp);
return *this;
}
_CMatrix inverted(bool * ok = 0) {_CMatrix tm(*this); tm.invert(ok); return tm;}
_CMatrix transposed() {_CMatrix tm(rows_, cols_); PIMM_FOR_WB(c, r) tm[r][c] = m[c][r]; return tm;}
_CMatrix transposed() {_CMatrix tm(_V2D::rows_, _V2D::cols_); PIMM_FOR(c, r) tm[c][r] = (*this)[r][c]; return tm;}
private:
uint cols_, rows_;
PIVector<PIVector<Type> > m;
// size_t rows_, cols_;
// PIVector<Type> mat;
};
#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[c][r]; 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 << '{'; for (uint r = 0; r < m.rows(); ++r) { for (uint c = 0; c < m.cols(); ++c) { s << m[c][r]; if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";} if (r < m.rows() - 1) s << PICoutManipulators::NewLine << ' ';} s << '}'; return s;}
inline PICout operator <<(PICout s, const PIMathMatrix<Type> & m) {s << '{'; for (uint r = 0; r < m.rows(); ++r) { for (uint c = 0; c < m.cols(); ++c) { s << m[r][c]; if (c < m.cols() - 1 || r < m.rows() - 1) s << ", ";} if (r < m.rows() - 1) s << PICoutManipulators::NewLine << ' ';} s << '}'; return s;}
/// Multiply matrices {CR x Rows0} on {Cols1 x CR}, result is {Cols1 x Rows0}
template<typename Type>
@@ -495,8 +528,8 @@ inline PIMathMatrix<Type> operator *(const PIMathMatrix<Type> & fm,
for (uint i = 0; i < cols1; ++i) {
t = Type(0);
for (uint k = 0; k < cr; ++k)
t += fm[k][j] * sm[i][k];
tm[i][j] = t;
t += fm[j][k] * sm[k][i];
tm[j][i] = t;
}
}
return tm;
@@ -522,11 +555,16 @@ inline PIMathVector<Type> operator *(const PIMathMatrix<Type> & fm,
typedef PIMathMatrix<int> PIMathMatrixi;
typedef PIMathMatrix<double> PIMathMatrixd;
#undef PIMV_FOR
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[r][c].imag(-(ret[r][c].imag()));
return ret.transposed();
}
#undef PIMM_FOR
#undef PIMM_FOR_WB
#undef PIMM_FOR_I
#undef PIMM_FOR_I_WB
#undef PIMM_FOR_A
#undef PIMM_FOR_C
#undef PIMM_FOR_R