1027 lines
43 KiB
C++
1027 lines
43 KiB
C++
/*! \file pimath.h
|
||
* \brief Many mathematical functions and classes
|
||
*/
|
||
/*
|
||
PIP - Platform Independent Primitives
|
||
Math
|
||
Copyright (C) 2014 Ivan Pelipenko peri4ko@gmail.com
|
||
|
||
This program is free software: you can redistribute it and/or modify
|
||
it under the terms of the GNU 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 General Public License for more details.
|
||
|
||
You should have received a copy of the GNU General Public License
|
||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
#ifndef PIMATH_H
|
||
#define PIMATH_H
|
||
|
||
#include "pibytearray.h"
|
||
#ifndef QNX
|
||
# include <complex>
|
||
# include <cmath>
|
||
#else
|
||
# include <complex.h>
|
||
# include <math.h>
|
||
# undef PIP_MATH_J0
|
||
# undef PIP_MATH_J1
|
||
# undef PIP_MATH_JN
|
||
# undef PIP_MATH_Y0
|
||
# undef PIP_MATH_Y1
|
||
# undef PIP_MATH_YN
|
||
#endif
|
||
|
||
#ifndef M_LN2
|
||
# define M_LN2 0.69314718055994530942
|
||
#endif
|
||
#ifndef M_LN10
|
||
# define M_LN10 2.30258509299404568402
|
||
#endif
|
||
#ifndef M_SQRT2
|
||
# define M_SQRT2 1.41421356237309514547
|
||
#endif
|
||
#ifndef M_SQRT3
|
||
# define M_SQRT3 1.73205080756887719318
|
||
#endif
|
||
#ifndef M_1_SQRT2
|
||
# define M_1_SQRT2 0.70710678118654746172
|
||
#endif
|
||
#ifndef M_1_SQRT3
|
||
# define M_1_SQRT3 0.57735026918962584208
|
||
#endif
|
||
#ifndef M_PI
|
||
# define M_PI 3.14159265358979323846
|
||
#endif
|
||
#ifndef M_2PI
|
||
# define M_2PI 6.28318530717958647692
|
||
#endif
|
||
#ifndef M_PI_3
|
||
# define M_PI_3 1.04719755119659774615
|
||
#endif
|
||
#ifndef M_2PI_3
|
||
# define M_2PI_3 2.0943951023931954923
|
||
#endif
|
||
#ifndef M_180_PI
|
||
# define M_180_PI 57.2957795130823208768
|
||
#endif
|
||
#ifndef M_PI_180
|
||
# define M_PI_180 1.74532925199432957692e-2
|
||
#endif
|
||
#ifndef M_E
|
||
# define M_E 2.7182818284590452353602874713527
|
||
#endif
|
||
#ifndef M_LIGHT_SPEED
|
||
# define M_LIGHT_SPEED 2.99792458e+8
|
||
#endif
|
||
|
||
using std::complex;
|
||
|
||
typedef complex<int> complexi;
|
||
typedef complex<float> complexf;
|
||
typedef complex<double> complexd;
|
||
typedef complex<ldouble> complexld;
|
||
const complexld complexld_i(0., 1.);
|
||
const complexld complexld_0(0.);
|
||
const complexld complexld_1(1.);
|
||
const complexd complexd_i(0., 1.);
|
||
const complexd complexd_0(0.);
|
||
const complexd complexd_1(1.);
|
||
|
||
__PICONTAINERS_SIMPLE_TYPE__(complexi)
|
||
__PICONTAINERS_SIMPLE_TYPE__(complexf)
|
||
__PICONTAINERS_SIMPLE_TYPE__(complexd)
|
||
__PICONTAINERS_SIMPLE_TYPE__(complexld)
|
||
|
||
const double deg2rad = M_PI_180;
|
||
const double rad2deg = M_180_PI;
|
||
|
||
inline int sign(const float & x) {return (x < 0.) ? -1 : (x > 0. ? 1 : 0);}
|
||
inline int sign(const double & x) {return (x < 0.) ? -1 : (x > 0. ? 1 : 0);}
|
||
inline complexd sign(const complexd & x) {return complexd(sign(x.real()), sign(x.imag()));}
|
||
inline int pow2(const int p) {return 1 << p;}
|
||
inline double sqr(const int v) {return v * v;}
|
||
inline double sqr(const float & v) {return v * v;}
|
||
inline double sqr(const double & v) {return v * v;}
|
||
inline double sinc(const double & v) {if (v == 0.) return 1.; double t = M_PI * v; return sin(t) / t;}
|
||
inline complexd round(const complexd & c) {return complexd(piRound<double>(c.real()), piRound<double>(c.imag()));}
|
||
inline complexd floor(const complexd & c) {return complexd(floor(c.real()), floor(c.imag()));}
|
||
inline complexd ceil(const complexd & c) {return complexd(ceil(c.real()), ceil(c.imag()));}
|
||
inline complexd atanc(const complexd & c) {return -complexd(-0.5, 1.) * log((complexd_1 + complexd_i * c) / (complexd_1 - complexd_i * c));}
|
||
inline complexd asinc(const complexd & c) {return -complexd_i * log(complexd_i * c + sqrt(complexd_1 - c * c));}
|
||
inline complexd acosc(const complexd & c) {return -complexd_i * log(c + complexd_i * sqrt(complexd_1 - c * c));}
|
||
#ifdef CC_GCC
|
||
# if CC_GCC_VERSION <= 0x025F
|
||
inline complexd tan(const complexd & c) {return sin(c) / cos(c);}
|
||
inline complexd tanh(const complexd & c) {return sinh(c) / cosh(c);}
|
||
inline complexd log2(const complexd & c) {return log(c) / M_LN2;}
|
||
inline complexd log10(const complexd & c) {return log(c) / M_LN10;}
|
||
# endif
|
||
#endif
|
||
double piJ0(const double & v);
|
||
double piJ1(const double & v);
|
||
double piJn(int n, const double & v);
|
||
double piY0(const double & v);
|
||
double piY1(const double & v);
|
||
double piYn(int n, const double & v);
|
||
inline double toDb(double val) {return 10. * log10(val);}
|
||
inline double fromDb(double val) {return pow(10., val / 10.);}
|
||
inline double toRad(double deg) {return deg * M_PI_180;}
|
||
inline double toDeg(double rad) {return rad * M_180_PI;}
|
||
|
||
template<typename T>
|
||
inline PICout operator <<(PICout s, const complex<T> & v) {s.space(); s.setControl(0, true); s << "(" << v.real() << "; " << v.imag() << ")"; s.restoreControl(); return s;}
|
||
|
||
// [-1 ; 1]
|
||
inline double randomd() {return (double)random() / RAND_MAX * 2. - 1.;}
|
||
// [-1 ; 1] normal
|
||
double randomn(double dv = 0., double sv = 1.);
|
||
|
||
inline PIVector<double> abs(const PIVector<complexd> & v) {
|
||
PIVector<double> result;
|
||
result.resize(v.size());
|
||
for (uint i = 0; i < v.size(); i++)
|
||
result[i] = abs(v[i]);
|
||
return result;
|
||
}
|
||
inline PIVector<double> abs(const PIVector<double> & v) {
|
||
PIVector<double> result;
|
||
result.resize(v.size());
|
||
for (uint i = 0; i < v.size(); i++)
|
||
result[i] = abs(v[i]);
|
||
return result;
|
||
}
|
||
|
||
template<uint Cols, uint Rows, typename Type>
|
||
class PIMathMatrixT;
|
||
|
||
/// Vector templated
|
||
|
||
#define PIMV_FOR(v, s) for (uint v = s; v < Size; ++v)
|
||
|
||
#pragma pack(push, 1)
|
||
template<uint Size, typename Type = double>
|
||
class PIP_EXPORT PIMathVectorT {
|
||
typedef PIMathVectorT<Size, Type> _CVector;
|
||
public:
|
||
PIMathVectorT() {resize(Size);}
|
||
//PIMathVectorT(Type val) {resize(Size); PIMV_FOR(i, 0) c[i] = val;}
|
||
PIMathVectorT(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);}
|
||
PIMathVectorT(const PIVector<Type> & val) {resize(Size); PIMV_FOR(i, 0) c[i] = val[i];}
|
||
PIMathVectorT(const _CVector & st, const _CVector & fn) {resize(Size); 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;}
|
||
Type angleCos(const _CVector & v) const {Type tv = v.length() * length(); return (tv == Type(0) ? Type(0) : ((*this) ^ v) / tv);}
|
||
Type angleSin(const _CVector & v) const {Type tv = angleCos(v); return sqrt(Type(1) - tv * tv);}
|
||
Type angleRad(const _CVector & v) const {return acos(angleCos(v));}
|
||
Type angleDeg(const _CVector & v) const {return toDeg(acos(angleCos(v)));}
|
||
_CVector projection(const _CVector & v) {Type tv = v.length(); return (tv == Type(0) ? _CVector() : v * (((*this) ^ v) / tv));}
|
||
_CVector & normalize() {Type tv = length(); if (tv == Type(1)) return *this; if (piAbs<Type>(tv) <= Type(1E-100)) {fill(Type(0)); return *this;} PIMV_FOR(i, 0) c[i] /= tv; return *this;}
|
||
_CVector normalized() {_CVector tv(*this); tv.normalize(); return tv;}
|
||
bool isNull() const {PIMV_FOR(i, 0) if (c[i] != Type(0)) return false; return true;}
|
||
bool isOrtho(const _CVector & v) const {return ((*this) ^ v) == Type(0);}
|
||
|
||
Type & at(uint index) {return c[index];}
|
||
Type at(uint index) const {return c[index];}
|
||
Type & operator [](uint index) {return c[index];}
|
||
Type operator [](uint index) const {return c[index];}
|
||
_CVector & operator =(const _CVector & v) {memcpy(c, v.c, sizeof(Type) * Size); return *this;}
|
||
bool operator ==(const _CVector & v) const {PIMV_FOR(i, 0) if (c[i] != v[i]) return false; return true;}
|
||
bool operator !=(const _CVector & v) const {return !(*this == c);}
|
||
void operator +=(const _CVector & v) {PIMV_FOR(i, 0) c[i] += v[i];}
|
||
void operator -=(const _CVector & v) {PIMV_FOR(i, 0) c[i] -= v[i];}
|
||
void operator *=(const Type & v) {PIMV_FOR(i, 0) c[i] *= v;}
|
||
void operator *=(const _CVector & v) {PIMV_FOR(i, 0) c[i] *= v[i];}
|
||
void operator /=(const Type & v) {PIMV_FOR(i, 0) c[i] /= v;}
|
||
void operator /=(const _CVector & v) {PIMV_FOR(i, 0) c[i] /= v[i];}
|
||
_CVector operator -() const {_CVector tv; PIMV_FOR(i, 0) tv[i] = -c[i]; return tv;}
|
||
_CVector operator +(const _CVector & v) const {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] += v[i]; return tv;}
|
||
_CVector operator -(const _CVector & v) const {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] -= v[i]; return tv;}
|
||
_CVector operator *(const Type & v) const {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] *= v; return tv;}
|
||
_CVector operator /(const Type & v) const {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] /= v; return tv;}
|
||
_CVector operator /(const _CVector & v) const {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] /= v[i]; return tv;}
|
||
_CVector operator *(const _CVector & v) const {if (Size > 3) return _CVector(); _CVector tv; tv.fill(Type(1)); tv[0] = c[1]*v[2] - v[1]*c[2]; tv[1] = v[0]*c[2] - c[0]*v[2]; tv[2] = c[0]*v[1] - v[0]*c[1]; return tv;}
|
||
Type operator ^(const _CVector & v) const {Type tv(0); PIMV_FOR(i, 0) tv += c[i] * v[i]; return tv;}
|
||
|
||
operator PIMathMatrixT<1, Size, Type>() {return PIMathMatrixT<1, Size, Type>(c);}
|
||
Type distToLine(const _CVector & lp0, const _CVector & lp1) {
|
||
_CVector a(lp0, lp1), b(lp0, *this), c(lp1, *this);
|
||
Type f = fabs(a[0]*b[1] - a[1]*b[0]) / a.length();//, s = b.length() + c.length() - a.length();
|
||
return f;}
|
||
|
||
template<uint Size1, typename Type1> /// vector {Size, Type} to vector {Size1, Type1}
|
||
PIMathVectorT<Size1, Type1> turnTo() {PIMathVectorT<Size1, Type1> tv; uint sz = piMin<uint>(Size, Size1); for (uint i = 0; i < sz; ++i) tv[i] = c[i]; return tv;}
|
||
|
||
private:
|
||
void resize(uint size, const Type & new_value = Type()) {s = size; for (int i = 0; i < s; ++i) c[i] = new_value;}
|
||
|
||
int s;
|
||
Type c[Size];
|
||
|
||
};
|
||
#pragma pack(pop)
|
||
|
||
template<uint Size, typename Type>
|
||
inline std::ostream & operator <<(std::ostream & s, const PIMathVectorT<Size, Type> & v) {s << '{'; PIMV_FOR(i, 0) {s << v[i]; if (i < Size - 1) s << ", ";} s << '}'; return s;}
|
||
template<uint Size, typename Type>
|
||
inline PICout operator <<(PICout s, const PIMathVectorT<Size, Type> & v) {s << '{'; PIMV_FOR(i, 0) {s << v[i]; if (i < Size - 1) s << ", ";} s << '}'; return s;}
|
||
template<uint Size, typename Type>
|
||
inline bool operator ||(const PIMathVectorT<Size, Type> & f, const PIMathVectorT<Size, Type> & s) {return (f * s).isNull();}
|
||
template<uint Size, typename Type>
|
||
inline PIMathVectorT<Size, Type> sqrt(const PIMathVectorT<Size, Type> & v) {PIMathVectorT<Size, Type> ret; PIMV_FOR(i, 0) {ret[i] = sqrt(v[i]);} return ret;}
|
||
template<uint Size, typename Type>
|
||
inline PIMathVectorT<Size, Type> sqr(const PIMathVectorT<Size, Type> & v) {PIMathVectorT<Size, Type> ret; PIMV_FOR(i, 0) {ret[i] = sqr(v[i]);} return ret;}
|
||
|
||
template<uint Size, typename Type>
|
||
inline PIByteArray & operator <<(PIByteArray & s, const PIMathVectorT<Size, Type> & v) {for (int i = 0; i < Size; ++i) s << v[i]; return s;}
|
||
template<uint Size, typename Type>
|
||
inline PIByteArray & operator >>(PIByteArray & s, PIMathVectorT<Size, Type> & v) {for (int i = 0; i < Size; ++i) s >> v[i]; return s;}
|
||
|
||
//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;}
|
||
|
||
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;
|
||
|
||
/// Matrix templated
|
||
|
||
#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)
|
||
|
||
#pragma pack(push, 1)
|
||
template<uint Cols, uint Rows = Cols, typename Type = double>
|
||
class PIP_EXPORT PIMathMatrixT {
|
||
typedef PIMathMatrixT<Cols, Rows, Type> _CMatrix;
|
||
typedef PIMathMatrixT<Rows, Cols, Type> _CMatrixI;
|
||
typedef PIMathVectorT<Rows, Type> _CMCol;
|
||
typedef PIMathVectorT<Cols, Type> _CMRow;
|
||
public:
|
||
PIMathMatrixT() {resize(Cols, Rows);}
|
||
PIMathMatrixT(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);}
|
||
PIMathMatrixT(const PIVector<Type> & val) {resize(Cols, Rows); int i = 0; PIMM_FOR_I_WB(c, r) m[c][r] = val[i++];}
|
||
|
||
static _CMatrix identity() {_CMatrix tm = _CMatrix(); PIMM_FOR_WB(c, r) tm.m[c][r] = (c == r ? Type(1) : Type(0)); 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();}
|
||
|
||
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 & 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;}
|
||
|
||
Type & at(uint col, uint row) {return m[col][row];}
|
||
Type at(uint col, uint row) const {return m[col][row];}
|
||
Type * operator [](uint col) {return m[col];}
|
||
const Type * operator [](uint col) const {return m[col];}
|
||
void operator =(const _CMatrix & sm) {memcpy(m, sm.m, sizeof(Type) * Cols * Rows);}
|
||
bool operator ==(const _CMatrix & sm) const {PIMM_FOR_WB(c, r) if (m[c][r] != sm.m[c][r]) 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; PIMM_FOR_WB(c, r) tm.m[c][r] = -m[c][r]; return tm;}
|
||
_CMatrix operator +(const _CMatrix & sm) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] += sm.m[c][r]; return tm;}
|
||
_CMatrix operator -(const _CMatrix & sm) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] -= sm.m[c][r]; return tm;}
|
||
_CMatrix operator *(const Type & v) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] *= v; return tm;}
|
||
_CMatrix operator /(const Type & v) {_CMatrix tm = _CMatrix(*this); PIMM_FOR_WB(c, r) tm.m[c][r] /= v; return tm;}
|
||
|
||
_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 j = 0; j < Cols; ++j) if (smat.m[j][i] != 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-100)) {
|
||
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) {
|
||
if (Cols != Rows) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
_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 j = 0; j < Cols; ++j) if (smat.m[j][i] != 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];
|
||
}
|
||
//cout << i << endl << smat << endl;
|
||
if (i < Cols - 1) {
|
||
if (fabs(smat.m[i+1][i+1]) < Type(1E-100)) {
|
||
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) {_CMatrix tm(*this); tm.invert(ok); return tm;}
|
||
_CMatrixI transposed() {_CMatrixI tm; PIMM_FOR_WB(c, r) tm[r][c] = m[c][r]; return tm;}
|
||
|
||
private:
|
||
void resize(uint cols_, uint rows_, const Type & new_value = Type()) {c_ = cols_; r_ = rows_; PIMM_FOR_WB(c, r) m[c][r] = new_value;}
|
||
int c_, r_;
|
||
Type m[Cols][Rows];
|
||
|
||
};
|
||
#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<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<uint Cols, uint Rows, typename Type>
|
||
inline std::ostream & operator <<(std::ostream & s, const PIMathMatrixT<Cols, Rows, Type> & m) {s << '{'; PIMM_FOR_I(c, r) s << m[c][r]; if (c < Cols - 1 || r < Rows - 1) s << ", ";} if (r < Rows - 1) s << endl << ' ';} s << '}'; return s;}
|
||
template<uint Cols, uint Rows, typename Type>
|
||
inline PICout operator <<(PICout s, const PIMathMatrixT<Cols, Rows, Type> & m) {s << '{'; PIMM_FOR_I(c, r) s << m[c][r]; if (c < Cols - 1 || r < Rows - 1) s << ", ";} if (r < Rows - 1) s << NewLine << ' ';} s << '}'; return s;}
|
||
|
||
/// Multiply matrices {CR x Rows0} on {Cols1 x CR}, result is {Cols1 x Rows0}
|
||
template<uint CR, uint Rows0, uint Cols1, typename Type>
|
||
inline PIMathMatrixT<Cols1, Rows0, Type> operator *(const PIMathMatrixT<CR, Rows0, Type> & fm,
|
||
const PIMathMatrixT<Cols1, CR, Type> & sm) {
|
||
PIMathMatrixT<Cols1, Rows0, 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[k][j] * sm[i][k];
|
||
tm[i][j] = t;
|
||
}
|
||
}
|
||
return tm;
|
||
}
|
||
|
||
/// Multiply matrix {Cols x Rows} on vector {Cols}, result is vector {Rows}
|
||
template<uint Cols, uint Rows, typename Type>
|
||
inline PIMathVectorT<Rows, Type> operator *(const PIMathMatrixT<Cols, Rows, Type> & fm,
|
||
const PIMathVectorT<Cols, Type> & sv) {
|
||
PIMathVectorT<Rows, Type> tv;
|
||
Type t;
|
||
for (uint i = 0; i < Rows; ++i) {
|
||
t = Type(0);
|
||
for (uint j = 0; j < Cols; ++j)
|
||
t += fm[j][i] * sv[j];
|
||
tv[i] = t;
|
||
}
|
||
return tv;
|
||
}
|
||
|
||
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 PIMV_FOR
|
||
#undef PIMM_FOR
|
||
#undef PIMM_FOR_WB
|
||
#undef PIMM_FOR_I
|
||
#undef PIMM_FOR_I_WB
|
||
#undef PIMM_FOR_C
|
||
#undef PIMM_FOR_R
|
||
|
||
/// Vector
|
||
|
||
#define PIMV_FOR(v, s) for (uint v = s; v < size_; ++v)
|
||
|
||
template<typename Type>
|
||
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];}
|
||
|
||
uint size() const {return size_;}
|
||
_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());}
|
||
Type manhattanLength() const {Type tv(0); PIMV_FOR(i, 0) tv += fabs(c[i]); return tv;}
|
||
Type angleCos(const _CVector & v) const {Type tv = v.length() * length(); return (tv == Type(0) ? Type(0) : ((*this) ^ v) / tv);}
|
||
Type angleSin(const _CVector & v) const {Type tv = angleCos(v); return sqrt(Type(1) - tv * tv);}
|
||
Type angleRad(const _CVector & v) const {return acos(angleCos(v));}
|
||
Type angleDeg(const _CVector & v) const {return toDeg(acos(angleCos(v)));}
|
||
_CVector projection(const _CVector & v) {Type tv = v.length(); return (tv == Type(0) ? _CVector() : v * (((*this) ^ v) / tv));}
|
||
_CVector & normalize() {Type tv = length(); if (tv == Type(1)) return *this; if (piAbs<Type>(tv) <= Type(1E-100)) {fill(Type(0)); return *this;} PIMV_FOR(i, 0) c[i] /= tv; return *this;}
|
||
_CVector normalized() {_CVector tv(*this); tv.normalize(); return tv;}
|
||
bool isNull() const {PIMV_FOR(i, 0) if (c[i] != Type(0)) return false; return true;}
|
||
bool isOrtho(const _CVector & v) const {return ((*this) ^ v) == Type(0);}
|
||
|
||
Type & at(uint index) {return c[index];}
|
||
Type at(uint index) const {return c[index];}
|
||
Type & operator [](uint index) {return c[index];}
|
||
Type operator [](uint index) const {return c[index];}
|
||
void operator =(const _CVector & v) {c = v.c;}
|
||
bool operator ==(const _CVector & v) const {PIMV_FOR(i, 0) if (c[i] != v[i]) return false; return true;}
|
||
bool operator !=(const _CVector & v) const {return !(*this == c);}
|
||
void operator +=(const _CVector & v) {PIMV_FOR(i, 0) c[i] += v[i];}
|
||
void operator -=(const _CVector & v) {PIMV_FOR(i, 0) c[i] -= v[i];}
|
||
void operator *=(const Type & v) {PIMV_FOR(i, 0) c[i] *= v;}
|
||
void operator *=(const _CVector & v) {PIMV_FOR(i, 0) c[i] *= v[i];}
|
||
void operator /=(const Type & v) {PIMV_FOR(i, 0) c[i] /= v;}
|
||
void operator /=(const _CVector & v) {PIMV_FOR(i, 0) c[i] /= v[i];}
|
||
_CVector operator -() {_CVector tv; PIMV_FOR(i, 0) tv[i] = -c[i]; return tv;}
|
||
_CVector operator +(const _CVector & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] += v[i]; return tv;}
|
||
_CVector operator -(const _CVector & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] -= v[i]; return tv;}
|
||
_CVector operator *(const Type & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] *= v; return tv;}
|
||
_CVector operator /(const Type & v) {_CVector tv = _CVector(*this); PIMV_FOR(i, 0) tv[i] /= v; return tv;}
|
||
_CVector operator *(const _CVector & v) {if (size_ < 3) return _CVector(); _CVector tv; tv.fill(Type(1)); tv[0] = c[1]*v[2] - v[1]*c[2]; tv[1] = v[0]*c[2] - c[0]*v[2]; tv[2] = c[0]*v[1] - v[0]*c[1]; return tv;}
|
||
Type operator ^(const _CVector & v) const {Type tv(0); PIMV_FOR(i, 0) tv += c[i] * v[i]; return tv;}
|
||
|
||
//inline operator PIMathMatrix<1, Size, Type>() {return PIMathMatrix<1, Size, Type>(c);}
|
||
Type distToLine(const _CVector & lp0, const _CVector & lp1) {
|
||
_CVector a(lp0, lp1), b(lp0, *this), c(lp1, *this);
|
||
Type f = fabs(a[0]*b[1] - a[1]*b[0]) / a.length();//, s = b.length() + c.length() - a.length();
|
||
return f;}
|
||
|
||
template<typename Type1>
|
||
PIMathVector turnTo(uint size) {PIMathVector<Type1> tv; uint sz = piMin<uint>(size_, size); for (uint i = 0; i < sz; ++i) tv[i] = c[i]; return tv;}
|
||
|
||
private:
|
||
uint size_;
|
||
PIVector<Type> c;
|
||
|
||
};
|
||
|
||
template<typename Type>
|
||
inline std::ostream & operator <<(std::ostream & s, const PIMathVector<Type> & v) {s << '{'; for (uint i = 0; i < v.size(); ++i) {s << v[i]; if (i < v.size() - 1) s << ", ";} s << '}'; return s;}
|
||
template<typename Type>
|
||
inline PICout operator <<(PICout s, const PIMathVector<Type> & v) {s << '{'; for (uint i = 0; i < v.size(); ++i) {s << v[i]; if (i < v.size() - 1) s << ", ";} s << '}'; return s;}
|
||
|
||
typedef PIMathVector<int> PIMathVectori;
|
||
typedef PIMathVector<double> PIMathVectord;
|
||
|
||
/// 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)
|
||
|
||
template<typename Type>
|
||
class PIP_EXPORT PIMathMatrix {
|
||
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++];}
|
||
|
||
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;}
|
||
|
||
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;}
|
||
|
||
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;}
|
||
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;}
|
||
|
||
_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 j = 0; j < cols_; ++j) if (smat.m[j][i] != 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-100)) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
}
|
||
}
|
||
if (ok != 0) *ok = true;
|
||
m = smat.m;
|
||
return *this;
|
||
}
|
||
|
||
_CMatrix & invert(bool * ok = 0, _CMCol * sv = 0) {
|
||
if (cols_ != rows_) {
|
||
if (ok != 0) *ok = false;
|
||
return *this;
|
||
}
|
||
_CMatrix mtmp = _CMatrix::identity(cols_, rows_), 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 j = 0; j < cols_; ++j) if (smat.m[j][i] != 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);
|
||
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];
|
||
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 (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;
|
||
if (sv != 0) (*sv)[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 (sv != 0) (*sv)[j] -= mul * (*sv)[i];
|
||
}
|
||
}
|
||
if (ok != 0) *ok = true;
|
||
m = mtmp.m;
|
||
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;}
|
||
|
||
private:
|
||
uint cols_, rows_;
|
||
PIVector<PIVector<Type> > m;
|
||
|
||
};
|
||
|
||
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 << endl << ' ';} s << '}'; return s;}
|
||
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 << NewLine << ' ';} s << '}'; 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[k][j] * sm[i][k];
|
||
tm[i][j] = 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 i = 0; i < r; ++i) {
|
||
t = Type(0);
|
||
for (uint j = 0; j < c; ++j)
|
||
t += fm[j][i] * sv[j];
|
||
tv[i] = t;
|
||
}
|
||
return tv;
|
||
}
|
||
|
||
typedef PIMathMatrix<int> PIMathMatrixi;
|
||
typedef PIMathMatrix<double> PIMathMatrixd;
|
||
|
||
#undef PIMV_FOR
|
||
#undef PIMM_FOR
|
||
#undef PIMM_FOR_WB
|
||
#undef PIMM_FOR_I
|
||
#undef PIMM_FOR_I_WB
|
||
#undef PIMM_FOR_C
|
||
#undef PIMM_FOR_R
|
||
|
||
|
||
/// Differential evaluations
|
||
|
||
struct TransferFunction { // Для задания передаточной функции
|
||
PIVector<double> vector_Bm, vector_An;
|
||
};
|
||
|
||
// Класс, служащий для перевода передаточной функции в систему ОДУ первого порядка
|
||
// реализованы след. методы решения дифф. ур-ний:
|
||
// Эйлера
|
||
// Рунге-Кутта 4-го порядка
|
||
// Адамса-Бэшфортса-Моултона 2, 3, 4 порядков
|
||
class PIP_EXPORT Solver
|
||
{
|
||
public:
|
||
enum Method {Global = -1,
|
||
Eyler_1 = 01,
|
||
Eyler_2 = 02,
|
||
EylerKoshi = 03,
|
||
RungeKutta_4 = 14,
|
||
AdamsBashfortMoulton_2 = 22,
|
||
AdamsBashfortMoulton_3 = 23,
|
||
AdamsBashfortMoulton_4 = 24,
|
||
PolynomialApproximation_2 = 32,
|
||
PolynomialApproximation_3 = 33,
|
||
PolynomialApproximation_4 = 34,
|
||
PolynomialApproximation_5 = 35
|
||
};
|
||
|
||
Solver() {times.resize(4); step = 0;}
|
||
|
||
void solve(double u, double h);
|
||
void fromTF(const TransferFunction & TF);
|
||
void setMethod(Method m) {method = m;}
|
||
void setTime(double time) {times.pop_back(); times.push_front(time);}
|
||
|
||
void solveEyler1(double u, double h);
|
||
void solveEyler2(double u, double h);
|
||
void solveRK4(double u, double h);
|
||
void solveABM2(double u, double h);
|
||
void solveABM3(double u, double h);
|
||
void solveABM4(double u, double h);
|
||
void solvePA(double u, double h, uint deg);
|
||
void solvePA2(double u, double h) {if (step > 0) solvePA(u, h, 2); else solveEyler1(u, h);}
|
||
void solvePA3(double u, double h) {if (step > 1) solvePA(u, h, 3); else solvePA2(u, h);}
|
||
void solvePA4(double u, double h) {if (step > 2) solvePA(u, h, 4); else solvePA3(u, h);}
|
||
void solvePA5(double u, double h) {if (step > 3) solvePA(u, h, 5); else solvePA4(u, h);}
|
||
|
||
PIMathVectord X;
|
||
static Method method_global;
|
||
static const char methods_desc[];
|
||
|
||
private:
|
||
void moveF() {for (uint i = F.size() - 1; i > 0; --i) F[i] = F[i - 1];}
|
||
|
||
PIMathMatrixd A, M;
|
||
PIMathVectord d, a1, b1;
|
||
PIMathVectord k1, k2, k3, k4, xx;
|
||
PIMathVectord XX, Y, pY;
|
||
PIVector<PIMathVectord> F;
|
||
PIVector<double> times;
|
||
uint size, step;
|
||
Method method;
|
||
double sum, td, ct, lp, dh, t, x1, x0;
|
||
bool ok;
|
||
|
||
};
|
||
|
||
|
||
|
||
class PIP_EXPORT PIFFT
|
||
{
|
||
public:
|
||
PIFFT();
|
||
// const PIVector<uint> & getIndexes() {return indexes;}
|
||
// const PIVector<complexd> & getCoefs() {return coefs;}
|
||
PIVector<complexd> * calcFFT(const PIVector<complexd> &val);
|
||
PIVector<complexd> * calcFFT(const PIVector<double> &val);
|
||
PIVector<complexd> * calcFFTinverse(const PIVector<complexd> &val);
|
||
PIVector<complexd> * calcHilbert(const PIVector<double> &val);
|
||
PIVector<double> getAmplitude();
|
||
private:
|
||
// PIVector<uint> indexes;
|
||
// PIVector<complexd> coefs;
|
||
PIVector<complexd> result;
|
||
// uint iterations;
|
||
bool prepared;
|
||
// uint out_size;
|
||
typedef ptrdiff_t ae_int_t;
|
||
void calc_coefs(uint cnt2);
|
||
void calc_indexes(uint cnt2, uint deep2);
|
||
complexd coef(uint n, uint k);
|
||
|
||
struct ftplan {
|
||
PIVector<int> plan;
|
||
PIVector<double> precomputed;
|
||
PIVector<double> tmpbuf;
|
||
PIVector<double> stackbuf;
|
||
};
|
||
|
||
ftplan curplan;
|
||
|
||
void fftc1d(const PIVector<complexd> &a, uint n);
|
||
void fftc1r(const PIVector<double> &a, uint n);
|
||
void fftc1dinv(const PIVector<complexd> &a, uint n);
|
||
|
||
void createPlan(uint n);
|
||
void ftbasegeneratecomplexfftplan(uint n, ftplan *plan);
|
||
void ftbase_ftbasegenerateplanrec(int n, int tasktype, ftplan *plan, int *plansize, int *precomputedsize, int *planarraysize, int *tmpmemsize, int *stackmemsize, ae_int_t stackptr, int debugi=0);
|
||
void ftbase_ftbaseprecomputeplanrec(ftplan *plan, int entryoffset, ae_int_t stackptr);
|
||
void ftbasefactorize(int n, int *n1, int *n2);
|
||
void ftbase_ftbasefindsmoothrec(int n, int seed, int leastfactor, int *best);
|
||
int ftbasefindsmooth(int n);
|
||
void ftbaseexecuteplan(PIVector<double> *a, int aoffset, int n, ftplan *plan);
|
||
void ftbaseexecuteplanrec(PIVector<double> *a, int aoffset, ftplan *plan, int entryoffset, ae_int_t stackptr);
|
||
void ftbase_internalcomplexlintranspose(PIVector<double> *a, int m, int n, int astart, PIVector<double> *buf);
|
||
void ftbase_ffticltrec(PIVector<double> *a, int astart, int astride, PIVector<double> *b, int bstart, int bstride, int m, int n);
|
||
void ftbase_internalreallintranspose(PIVector<double> *a, int m, int n, int astart, PIVector<double> *buf);
|
||
void ftbase_fftirltrec(PIVector<double> *a, int astart, int astride, PIVector<double> *b, int bstart, int bstride, int m, int n);
|
||
void ftbase_ffttwcalc(PIVector<double> *a, int aoffset, int n1, int n2);
|
||
};
|
||
|
||
|
||
template <typename T>
|
||
class PIP_EXPORT PIStatistic {
|
||
public:
|
||
PIStatistic() {
|
||
mean = T();
|
||
variance = T();
|
||
skewness = T();
|
||
kurtosis = T();
|
||
}
|
||
bool calculate(const PIVector<T> &val) {
|
||
T v = T(), v1 = T(), v2 = T(), stddev = T();
|
||
int i, n = val.size();
|
||
mean = T();
|
||
variance = T();
|
||
skewness = T();
|
||
kurtosis = T();
|
||
if (n < 2)
|
||
return false;
|
||
/*
|
||
* Mean
|
||
*/
|
||
for (i = 0; i < n; i++)
|
||
mean += val[i];
|
||
mean /= n;
|
||
/*
|
||
* Variance (using corrected two-pass algorithm)
|
||
*/
|
||
for (i = 0; i < n; i++) {
|
||
v1 += sqr(val[i] - mean);
|
||
}
|
||
for (i = 0; i < n; i++)
|
||
v2 += val[i] - mean;
|
||
v2 = sqr(v2) / n;
|
||
variance = (v1 - v2) / (n - 1);
|
||
if (variance < T())
|
||
variance = T();
|
||
stddev = sqrt(variance);
|
||
/*
|
||
* Skewness and kurtosis
|
||
*/
|
||
if (stddev != T()) {
|
||
for (i = 0; i < n; i++) {
|
||
v = (val[i] - mean) / stddev;
|
||
v2 = sqr(v);
|
||
skewness = skewness + v2 * v;
|
||
kurtosis = kurtosis + sqr(v2);
|
||
}
|
||
skewness /= n;
|
||
kurtosis = kurtosis / n - 3.;
|
||
}
|
||
return true;
|
||
}
|
||
T mean;
|
||
T variance;
|
||
T skewness;
|
||
T kurtosis;
|
||
};
|
||
|
||
typedef PIStatistic<int> PIStatistici;
|
||
typedef PIStatistic<float> PIStatisticf;
|
||
typedef PIStatistic<double> PIStatisticd;
|
||
|
||
|
||
template <typename T>
|
||
bool OLS_Linear(const PIVector<PIPair<T, T> > & input, T * out_a, T * out_b) {
|
||
if (input.size_s() < 2)
|
||
return false;
|
||
int n = input.size_s();
|
||
T a_t0 = T(), a_t1 = T(), a_t2 = T(), a_t3 = T(), a_t4 = T(), a = T(), b = T();
|
||
for (int i = 0; i < n; ++i) {
|
||
const PIPair<T, T> & cv(input[i]);
|
||
a_t0 += cv.first * cv.second;
|
||
a_t1 += cv.first;
|
||
a_t2 += cv.second;
|
||
a_t3 += cv.first * cv.first;
|
||
}
|
||
a_t4 = n * a_t3 - a_t1 * a_t1;
|
||
if (a_t4 != T())
|
||
a = (n * a_t0 - a_t1 * a_t2) / a_t4;
|
||
b = (a_t2 - a * a_t1) / n;
|
||
if (out_a != 0) *out_a = a;
|
||
if (out_b != 0) *out_b = b;
|
||
return true;
|
||
}
|
||
|
||
|
||
template <typename T>
|
||
bool WLS_Linear(const PIVector<PIPair<T, T> > & input, const PIVector<T> & weights, T * out_a, T * out_b) {
|
||
if (input.size_s() < 2)
|
||
return false;
|
||
if (input.size_s() != weights.size_s())
|
||
return false;
|
||
int n = input.size_s();
|
||
T a_t0 = T(), a_t1 = T(), a_t2 = T(), a_t3 = T(), a_t4 = T(), a_n = T(), a = T(), b = T();
|
||
for (int i = 0; i < n; ++i) {
|
||
T cp = weights[i];
|
||
const PIPair<T, T> & cv(input[i]);
|
||
a_t0 += cv.first * cv.second * cp;
|
||
a_t1 += cv.first * cp;
|
||
a_t2 += cv.second * cp;
|
||
a_t3 += cv.first * cv.first * cp;
|
||
a_n += cp;
|
||
}
|
||
a_t4 = a_n * a_t3 - a_t1 * a_t1;
|
||
if (a_t4 != T())
|
||
a = (a_n * a_t0 - a_t1 * a_t2) / a_t4;
|
||
b = (a_t2 - a * a_t1) / a_n;
|
||
if (out_a != 0) *out_a = a;
|
||
if (out_b != 0) *out_b = b;
|
||
return true;
|
||
}
|
||
|
||
#endif // PIMATH_H
|