diff --git a/src_main/containers/pivector2d.h b/src_main/containers/pivector2d.h index 3a58590d..3f929ac4 100644 --- a/src_main/containers/pivector2d.h +++ b/src_main/containers/pivector2d.h @@ -93,6 +93,7 @@ public: p_->copyRow(st_, other.data(), sz); return *this; } + inline PIVector toVector() const {return PIVector(p_->mat.data(st_), p_->cols_);} }; class RowConst { @@ -105,6 +106,7 @@ public: inline size_t size() const {return p_->cols_;} inline const T & operator [](size_t index) const {return p_->mat[st_ + index];} inline const T * data(size_t index = 0) const {return p_->mat.data(st_ + index);} + inline PIVector toVector() const {return PIVector(p_->mat.data(st_), p_->cols_);} }; @@ -114,6 +116,12 @@ public: inline const T * data(size_t index = 0) const {return mat.data(index);} inline Row row(size_t index) {return Row(this, index);} + inline PIVector col(size_t index) { + PIVector ret; + ret.reserve(rows_); + for (int i=0; i & setRow(size_t row, const Row & other) { size_t sz = piMin(cols_, other.p_->cols_); copyRow(cols_ * row, other.data(), sz); @@ -135,18 +143,25 @@ public: PIVector & plainVector() {return mat;} const PIVector & plainVector() const {return mat;} + inline void swap(PIVector2D & other) { + mat.swap(other.mat); + piSwap(rows_, other.rows_); + piSwap(cols_, other.cols_); + } + inline PIVector2D & _resizeRaw(size_t r, size_t c) { piCout << "Error, \"resizeRaw()\" only allowed for simple type declared with __PIVECTOR_SIMPLE_TYPE__ macro!"; assert(0); return *this; } -private: +protected: inline void copyRow(size_t start, const T * data, size_t size) { for (size_t i = 0; i < size; i++) mat[start + i] = data[i]; } +//private: size_t rows_, cols_; PIVector mat; }; diff --git a/src_main/math/pimathmatrix.h b/src_main/math/pimathmatrix.h index c5c535fb..9382bfd6 100644 --- a/src_main/math/pimathmatrix.h +++ b/src_main/math/pimathmatrix.h @@ -24,6 +24,24 @@ #define PIMATHMATRIX_H #include "pimathvector.h" +#include "pimathcomplex.h" + + + +template +inline bool _PIMathMatrixNullCompare(const T v) { + return (piAbs(v) < T(1E-100)); +} + +template<> +inline bool _PIMathMatrixNullCompare(const complexf v) { + return (abs(v) < float(1E-100)); +} +template<> +inline bool _PIMathMatrixNullCompare(const complexd v) { + return (abs(v) < double(1E-100)); +} + /// Matrix templated @@ -43,7 +61,6 @@ class PIP_EXPORT PIMathMatrixT { typedef PIMathVectorT _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 & 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 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 -class PIP_EXPORT PIMathMatrix { +class PIP_EXPORT PIMathMatrix : public PIVector2D { + typedef PIVector2D _V2D; typedef PIMathMatrix _CMatrix; typedef PIMathVector _CMCol; typedef PIMathVector _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 & 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 & val) {resize(cols, rows); int i=0; PIMM_FOR_I(c, r) (*this)[r][c] = val[i++];} + PIMathMatrix(const PIVector > & 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 & 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 & val) {return _CMatrix(val.size(), 1, val.toVector());} + static _CMatrix matrixCol(const PIMathVector & 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 & operator [](uint col) {return m[col];} - PIVector 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 > & v) {*this = PIVector2D(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::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 > m; - +// size_t rows_, cols_; +// PIVector mat; }; + #ifdef PIP_STD_IOSTREAM template inline std::ostream & operator <<(std::ostream & s, const PIMathMatrix & 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 -inline PICout operator <<(PICout s, const PIMathMatrix & 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 & 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 @@ -495,8 +528,8 @@ inline PIMathMatrix operator *(const PIMathMatrix & 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 operator *(const PIMathMatrix & fm, typedef PIMathMatrix PIMathMatrixi; typedef PIMathMatrix PIMathMatrixd; -#undef PIMV_FOR +template +PIMathMatrix > hermitian(const PIMathMatrix > & m) { + PIMathMatrix > 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 diff --git a/src_main/math/pimathvector.h b/src_main/math/pimathvector.h index 9f67ef1d..ca509d8c 100644 --- a/src_main/math/pimathvector.h +++ b/src_main/math/pimathvector.h @@ -24,10 +24,13 @@ #define PIMATHVECTOR_H #include "pimathbase.h" -#include template class PIMathMatrixT; +//class PIMathVectorT; + + +//#define PIMVCONSTR(n, t, args) PIMathVectorT _PIMathVectorConstruct(args) {PIVector v; for (int i=0; i(v)} /// Vector templated @@ -41,17 +44,14 @@ class PIP_EXPORT PIMathVectorT { public: PIMathVectorT() {resize();} //PIMathVectorT(Type val) {resize(Size); PIMV_FOR(i, 0) c[i] = val;} - PIMathVectorT(Type fval, ...) {resize(); c[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] = va_arg(vl, Type); va_end(vl);} PIMathVectorT(const PIVector & val) {resize(); PIMV_FOR(i, 0) c[i] = val[i];} PIMathVectorT(const _CVector & st, const _CVector & fn) {resize(); set(st, fn);} uint size() const {return Size;} _CVector & fill(const Type & v) {PIMV_FOR(i, 0) c[i] = v; return *this;} - _CVector & set(Type fval, ...) {c[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] = va_arg(vl, Type); va_end(vl); return *this;} _CVector & set(const _CVector & st, const _CVector & fn) {PIMV_FOR(i, 0) c[i] = fn[i] - st[i]; return *this;} _CVector & move(const Type & v) {PIMV_FOR(i, 0) c[i] += v; return *this;} _CVector & move(const _CVector & v) {PIMV_FOR(i, 0) c[i] += v[i]; return *this;} - _CVector & move(Type fval, ...) {c[0] += fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] += va_arg(vl, Type); va_end(vl); return *this;} Type lengthSqr() const {Type tv(0); PIMV_FOR(i, 0) tv += (c[i] * c[i]); return tv;} Type length() const {return sqrt(lengthSqr());} Type manhattanLength() const {Type tv(0); PIMV_FOR(i, 0) tv += fabs(c[i]); return tv;} @@ -145,12 +145,29 @@ inline PIByteArray & operator >>(PIByteArray & s, PIMathVectorT & v) //template /// vector {Size0, Type0} to vector {Size1, Type1} //inline operator PIMathVectorT(const PIMathVectorT & v) {PIMathVectorT tv; uint sz = piMin(Size0, Size1); for (uint i = 0; i < sz; ++i) tv[i] = v[i]; return tv;} + +template +inline PIMathVectorT<2u, T> createVectorT2(T x, T y) {return PIMathVectorT<2u, T>(PIVector() << x << y);} +template +inline PIMathVectorT<3u, T> createVectorT3(T x, T y, T z) {return PIMathVectorT<3u, T>(PIVector() << x << y << z);} +template +inline PIMathVectorT<4u, T> createVectorT4(T x, T y, T z, T w) {return PIMathVectorT<4u, T>(PIVector() << x << y << z << w);} + typedef PIMathVectorT<2u, int> PIMathVectorT2i; typedef PIMathVectorT<3u, int> PIMathVectorT3i; typedef PIMathVectorT<4u, int> PIMathVectorT4i; typedef PIMathVectorT<2u, double> PIMathVectorT2d; typedef PIMathVectorT<3u, double> PIMathVectorT3d; typedef PIMathVectorT<4u, double> PIMathVectorT4d; +#define createVectorT2i createVectorT2 +#define createVectorT3i createVectorT3 +#define createVectorT4i createVectorT4 +#define createVectorT2f createVectorT2 +#define createVectorT3f createVectorT3 +#define createVectorT4f createVectorT4 +#define createVectorT2d createVectorT2 +#define createVectorT3d createVectorT3 +#define createVectorT4d createVectorT4 #undef PIMV_FOR @@ -163,7 +180,6 @@ class PIP_EXPORT PIMathVector { typedef PIMathVector _CVector; public: PIMathVector(const uint size = 3) {resize(size);} - PIMathVector(const uint size, Type fval, ...) {resize(size); c[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] = va_arg(vl, Type); va_end(vl);} PIMathVector(const PIVector & val) {resize(val.size()); PIMV_FOR(i, 0) c[i] = val[i];} PIMathVector(const _CVector & st, const _CVector & fn) {resize(st.size()); PIMV_FOR(i, 0) c[i] = fn[i] - st[i];} @@ -171,10 +187,8 @@ public: _CVector & resize(uint size, const Type & new_value = Type()) {size_ = size; c.resize(size, new_value); return *this;} _CVector resized(uint size, const Type & new_value = Type()) {_CVector tv = _CVector(*this); tv.resize(size, new_value); return tv;} _CVector & fill(const Type & v) {PIMV_FOR(i, 0) c[i] = v; return *this;} - _CVector & set(Type fval, ...) {c[0] = fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] = va_arg(vl, Type); va_end(vl); return *this;} _CVector & move(const Type & v) {PIMV_FOR(i, 0) c[i] += v; return *this;} _CVector & move(const _CVector & v) {PIMV_FOR(i, 0) c[i] += v[i]; return *this;} - _CVector & move(Type fval, ...) {c[0] += fval; va_list vl; va_start(vl, fval); PIMV_FOR(i, 1) c[i] += va_arg(vl, Type); va_end(vl); return *this;} _CVector & swap(uint fe, uint se) {piSwap(c[fe], c[se]); return *this;} Type lengthSqr() const {Type tv(0); PIMV_FOR(i, 0) tv += (c[i] * c[i]); return tv;} Type length() const {return sqrt(lengthSqr());} @@ -219,6 +233,7 @@ public: template PIMathVector turnTo(uint size) const {PIMathVector tv; uint sz = piMin(size_, size); for (uint i = 0; i < sz; ++i) tv[i] = c[i]; return tv;} + PIVector toVector() const {return c;} private: uint size_; diff --git a/src_main/math/piquaternion.cpp b/src_main/math/piquaternion.cpp index fb0dccf8..fefd0306 100644 --- a/src_main/math/piquaternion.cpp +++ b/src_main/math/piquaternion.cpp @@ -21,7 +21,7 @@ PIMathVectorT3d PIQuaternion::eyler() const { angle_z = atan2(-rmat[0][1] / c, rmat[0][0] / c); } if (angle_z < 0) angle_z = 2*M_PI + angle_z; - return PIMathVectorT3d(angle_x,angle_y,angle_z); + return createVectorT3d(angle_x,angle_y,angle_z); } @@ -136,11 +136,10 @@ PIQuaternion PIQuaternion::fromAngles(double ax, double ay, double az) { res.q[2] = y; res.q[3] = z; return res; - return PIQuaternion(PIMathVectorT3d(x,y,z),a); } PIQuaternion PIQuaternion::fromAngles2(double ax, double ay, double az) { - double om = PIMathVectorT3d(ax,ay,az).length(); + double om = createVectorT3d(ax,ay,az).length(); // az = M_PI - az; if (om == 0.) return PIQuaternion(PIMathVectorT3d(), 1.); PIQuaternion res; diff --git a/src_main/math/piquaternion.h b/src_main/math/piquaternion.h index c58c9940..3b9ad156 100644 --- a/src_main/math/piquaternion.h +++ b/src_main/math/piquaternion.h @@ -39,7 +39,7 @@ public: double & scalar() {return q[0];} double scalar() const {return q[0];} - PIMathVectorT3d vector() const {return PIMathVectorT3d(q[1], q[2], q[3]);} + PIMathVectorT3d vector() const {return createVectorT3(q[1], q[2], q[3]);} PIMathVectorT3d eyler() const; PIMathMatrixT33d rotationMatrix() const;