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

17
src_main/containers/pivector2d.h
View File

@@ -93,6 +93,7 @@ public:
p_->copyRow(st_, other.data(), sz);
return *this;
}
inline PIVector<T> toVector() const {return PIVector<T>(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<T> toVector() const {return PIVector<T>(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<T> col(size_t index) {
PIVector<T> ret;
ret.reserve(rows_);
for (int i=0; i<rows_; i++) ret << mat[i*cols_+index];
return ret;
}
inline PIVector2D<T> & setRow(size_t row, const Row & other) {
size_t sz = piMin<size_t>(cols_, other.p_->cols_);
copyRow(cols_ * row, other.data(), sz);
@@ -135,18 +143,25 @@ public:
PIVector<T> & plainVector() {return mat;}
const PIVector<T> & plainVector() const {return mat;}
inline void swap(PIVector2D<T> & other) {
mat.swap(other.mat);
piSwap<size_t>(rows_, other.rows_);
piSwap<size_t>(cols_, other.cols_);
}
inline PIVector2D<T> & _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<T> mat;
};

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

29
src_main/math/pimathvector.h
View File

@@ -24,10 +24,13 @@
#define PIMATHVECTOR_H
#include "pimathbase.h"
#include <stdarg.h>
template<uint Cols, uint Rows, typename Type>
class PIMathMatrixT;
//class PIMathVectorT;
//#define PIMVCONSTR(n, t, args) PIMathVectorT<uint(n), t> _PIMathVectorConstruct(args) {PIVector<t> v; for (int i=0; i<n; ++i) v << a3; return PIMathVectorT<uint(n), t>(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<Type> & 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<Size, Type> & v)
//template<uint Size0, typename Type0 = double, uint Size1 = Size0, typename Type1 = Type0> /// vector {Size0, Type0} to vector {Size1, Type1}
//inline operator PIMathVectorT<Size1, Type1>(const PIMathVectorT<Size0, Type0> & v) {PIMathVectorT<Size1, Type1> tv; uint sz = piMin<uint>(Size0, Size1); for (uint i = 0; i < sz; ++i) tv[i] = v[i]; return tv;}
template<typename T>
inline PIMathVectorT<2u, T> createVectorT2(T x, T y) {return PIMathVectorT<2u, T>(PIVector<T>() << x << y);}
template<typename T>
inline PIMathVectorT<3u, T> createVectorT3(T x, T y, T z) {return PIMathVectorT<3u, T>(PIVector<T>() << x << y << z);}
template<typename T>
inline PIMathVectorT<4u, T> createVectorT4(T x, T y, T z, T w) {return PIMathVectorT<4u, T>(PIVector<T>() << 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<int>
#define createVectorT3i createVectorT3<int>
#define createVectorT4i createVectorT4<int>
#define createVectorT2f createVectorT2<float>
#define createVectorT3f createVectorT3<float>
#define createVectorT4f createVectorT4<float>
#define createVectorT2d createVectorT2<double>
#define createVectorT3d createVectorT3<double>
#define createVectorT4d createVectorT4<double>
#undef PIMV_FOR
@@ -163,7 +180,6 @@ class PIP_EXPORT PIMathVector {
typedef PIMathVector<Type> _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<Type> & 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<Type>(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<typename Type1>
PIMathVector turnTo(uint size) const {PIMathVector<Type1> tv; uint sz = piMin<uint>(size_, size); for (uint i = 0; i < sz; ++i) tv[i] = c[i]; return tv;}
PIVector<Type> toVector() const {return c;}
private:
uint size_;

5
src_main/math/piquaternion.cpp
View File

@@ -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;

2
src_main/math/piquaternion.h
View File

@@ -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<double>(q[1], q[2], q[3]);}
PIMathVectorT3d eyler() const;
PIMathMatrixT33d rotationMatrix() const;