/*! @file @id $Id$ */ // 1 2 3 4 5 6 7 8 // 45678901234567890123456789012345678901234567890123456789012345678901234567890 #include #include #include #include #include #include #include #include #include #include /** @mainpage @description @readme */ /// Auxiliary Mathematical Functions namespace math { /// Compare Floating Points Whether They Are Almost Equal /** Floating points such as @c float and @c double are not 100% exact, because the numbers are represented by a limited number of bits. That's why floating points should not be compared with normal equality operator @c ==, but use function math::equal. This function detects floating points and then calls almostEqual instead of @c ==. */ template bool almostEqual(TYPE a, TYPE b) { a = std::fabs(a); b = std::fabs(b); TYPE diff(std::fabs(a-b)); TYPE max(a>b?a:b); if (max<1) return diff<=1000*std::numeric_limits::epsilon(); return diff<=max*1000*std::numeric_limits::epsilon(); } /// Check Two Values For Equality /** If the values are floating point variables, it calls math::aux::almostEqual. */ template bool equal(const TYPE& a, const TYPE& b) { return a==b; } /// Check if Two long double Values are Nearly Equal /** calls math::aux::almostEqual. */ template<> bool equal(const long double& a, const long double& b) { return almostEqual(a, b); } /// Check if Two @c double Values are Nearly Equal /** calls math::aux::almostEqual. */ template<> bool equal(const double& a, const double& b) { return almostEqual(a, b); } /// Check if Two @c float Values are Nearly Equal /** calls math::aux::almostEqual. */ template<> bool equal(const float& a, const float& b) { return almostEqual(a, b); } } /** Base class with common functions for Matrix and Matrix. Implements generic common methods. */ template class MatrixBase { //..............................................................variables protected: size_t ROWS; size_t COLUMNS; size_t SIZE; size_t MEM_SIZE; //...............................................................typedefs public: /// @name Auxiliary Classes ///@{ /// Return One Row as Vector, internally used for element access /** Only used to access values: @code Matrix m; m[2][2] = 1; @endcode */ class RowVector { public: /// Get Column given a Matrix Row TYPE& operator[](size_t column) { assert(column<_m.COLUMNS); return _v[column]; } protected: friend class MatrixBase; RowVector() = delete; // forbidden RowVector(const MatrixBase& m, TYPE c[]): _m(m), _v(c) {} const MatrixBase& _m; TYPE *_v; }; /// Same as RowVector, but in a constant environment. class ConstRowVector { public: /// Get Column given a Matrix Row const TYPE& operator[](size_t column) const { assert(column<_m.COLUMNS); return _v[column]; } protected: friend class MatrixBase; ConstRowVector() = delete; // forbidden ConstRowVector(const MatrixBase& m, const TYPE c[]): _m(m), _v(c) {} const MatrixBase& _m; const TYPE *_v; }; ///@} //................................................................methods public: /// @name construction ///@{ MatrixBase(size_t rows, size_t columns): ROWS(rows), COLUMNS(columns), SIZE(ROWS*COLUMNS), MEM_SIZE(SIZE*sizeof(TYPE)) { } template MatrixBase(size_t rows, size_t columns, ARGS...t): ROWS(rows), COLUMNS(columns), SIZE(ROWS*COLUMNS), MEM_SIZE(SIZE*sizeof(TYPE)), _c{std::forward(t)...} { } ///@} /// @name element access ///@{ /// Access Matrix Element at Given Row and Column /** You have three possibilities to access an element of a matrix: @code Matrix m; int a21 = m[2][1]; // use bracket operator int b21 = m(2, 1); // use function operator int c21 = m.at(2, 1); // use at @endcode */ TYPE& at(size_t row, size_t column) { assert(row(TYPE*)(_c)) if (!math::equal(*--to, *--from)) return false; return true; } /// Compare To Other Matrix bool operator!=(const MatrixBase& o) const { return !operator==(o); } /// Add Other Matrix MatrixBase& operator+=(const MatrixBase& o) { assert_check(o); TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE); while (to>(TYPE*)(_c)) *--to += *--from; return *this; } /// Subtract Other Matrix MatrixBase& operator-=(const MatrixBase& o) { assert_check(o); TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE); while (to>(TYPE*)(_c)) *--to -= *--from; return *this; } /// Multiply Matrix With Scalar MatrixBase& operator*=(const TYPE& o) { TYPE *res((TYPE*)(_c)+SIZE); while (res>(TYPE*)(_c)) *--res *= o; return *this; } /// Divide Matrix By Scalar MatrixBase& operator/=(const TYPE& o) { TYPE *res((TYPE*)(_c)+SIZE); while (res>(TYPE*)(_c)) *--res /= o; return *this; } ///@} /// @name special operations ///@{ /// Apply Any External Function To Each Element MatrixBase& apply(std::function fn) { TYPE *to((TYPE*)(_c)+SIZE); while (to>(TYPE*)(_c)) fn(*--to); return *this; } /// Calculate Determinant Of The Matrix /** The Matrix is replaced by it's gaussian representation. */ TYPE det() { TYPE res(gauss()); for (TYPE *p((TYPE*)(_c)+SIZE); --p>=(TYPE*)(_c); p-=COLUMNS) res *= *p; return res; } /// Calculate Gaussian Representation /** The Matrix is replaced by it's gaussian representation. */ TYPE gauss() { /// calculate using gauss algorithmus /// @see http://www.mathebibel.de/determinante-berechnen-nach-gauss /// 1. normalize first line to first value TYPE lambda(at(0, 0)); if (lambda==0) { feraiseexcept(FE_DIVBYZERO); throw std::range_error("gauss calculation failed"); } at(0, 0) = 1; for (TYPE *p((TYPE*)(_c)+COLUMNS); p>(TYPE*)(_c)+1;) *--p/=lambda; /// 2. nullify lower triangle for (size_t column(0); column class Matrix: public MatrixBase { //...............................................................typedefs private: typedef MatrixBase Parent; //................................................................methods public: /// @name construction ///@{ Matrix(): Parent(TROWS, TCOLUMNS) { memset(Parent::_c, 0, Parent::MEM_SIZE); } Matrix(const Matrix& o): Matrix() { memcpy(Parent::_c, o._c, Parent::MEM_SIZE); } template Matrix(ARGS...t): Parent(TROWS, TCOLUMNS, t...) { static_assert(sizeof...(t)==TROWS*TCOLUMNS, "wrong array size"); } ///@} /// @name operators ///@{ Matrix& operator=(const Matrix& o) { Parent::operator=(o); return *this; } Matrix& operator+=(const Matrix& o) { Parent::operator+=(o); return *this; } Matrix& operator-=(const Matrix& o) { Parent::operator-=(o); return *this; } Matrix& operator*=(const TYPE& o) { Parent::operator*=(o); return *this; } Matrix& operator/=(const TYPE& o) { Parent::operator/=(o); return *this; } Matrix operator-() const { Matrix res(*this); for (TYPE *to((TYPE*)(res._c)+this->SIZE); to>(TYPE*)(res._c); *--to = -*to); return res; } template Matrix operator*(const Matrix& o) const { Matrix res; for (size_t i(0); iat(i, j) * o(j, k); return res; } ///@} /// @name special operations ///@{ Matrix& apply(std::function fn) { Parent::apply(fn); return *this; } Matrix t() const { Matrix res; for (size_t row(0); rowat(row, column); return res; } static Matrix i() { Matrix res; for (size_t row(0); rowCOLUMNS; ++column) { if (columnROWS) { /// 2. normalize pivot to one TYPE pivot(o(column, column)); if (pivot!=1) { o(column, column) = 1; for (size_t pos(column+1); posCOLUMNS; ++pos) o(column, pos)/=pivot; for (size_t pos(0); posCOLUMNS; ++pos) this->at(column, pos)/=pivot; } /// 3. nullify lower triangle for (size_t row(column+1); rowROWS; ++row) { TYPE pivot(o(row, column)); if (pivot!=0) { o(row, column) = 0; for (size_t pos(column+1); posCOLUMNS; ++pos) o(row, pos) -= pivot*o(column, pos); for (size_t pos(0); posCOLUMNS; ++pos) this->at(row, pos) -= pivot*this->at(column, pos); } } } } /// 4. nullify the upper triangle const size_t LASTCOL(this->COLUMNS-1); const size_t LASTROW(this->ROWS-1); for (size_t column(1); columnCOLUMNS; ++column) { for (size_t row(0); rowCOLUMNS; ++pos) o(row, pos) -= pivot*o(column, pos); for (size_t pos(0); posCOLUMNS; ++pos) this->at(row, pos) -= pivot*this->at(column, pos); } } } return *this; } ///@} }; //============================================================================== template class Matrix: public MatrixBase { //...............................................................typedefs private: typedef MatrixBase Parent; //................................................................methods public: /// @name construction ///@{ Matrix() = delete; Matrix(size_t rows, size_t columns): Parent(rows, columns) { assert(rows>0); assert(columns>0); Parent::_c = new TYPE[rows*columns]; memset(Parent::_c, 0, Parent::MEM_SIZE); } Matrix(const Matrix& o): Matrix(o.ROWS, o.Parent::COLUMNS) { memcpy(Parent::_c, o.Parent::_c, Parent::MEM_SIZE); } template Matrix(size_t rows, size_t columns, ARGS...t): Matrix(rows, columns) { assert(sizeof...(t)==Parent::SIZE); copy_args(Parent::_c, t...); } ///@} /// @name destruction ///@{ virtual ~Matrix() { delete[] Parent::_c; } ///@} /// @name operators ///@{ Matrix& operator=(const Matrix& o) { Parent::operator=(o); return *this; } Matrix& operator+=(const Matrix& o) { Parent::operator+=(o); return *this; } Matrix& operator-=(const Matrix& o) { Parent::operator-=(o); return *this; } Matrix& operator*=(const TYPE& o) { Parent::operator*=(o); return *this; } Matrix& operator/=(const TYPE& o) { Parent::operator/=(o); return *this; } Matrix operator-() const { Matrix res(*this); for (TYPE *to((TYPE*)(res._c)+this->SIZE); to>(TYPE*)(res._c); *--to = -*to); return res; } Matrix operator*(const Matrix& o) const { Matrix res(this->ROWS, o.COLUMNS); assert(this->COLUMNS==o.ROWS); for (size_t i(0); iROWS; ++i) for (size_t k(0); kCOLUMNS; ++j) res(i, k) += this->at(i, j) * o(j, k); return res; } ///@} ///@name special operations ///@{ Matrix& resize(size_t rows, size_t columns) { if (rows!=this->ROWS||columns!=this->COLUMNS) { delete Parent::_c; Parent::_c = new TYPE[rows*columns]; this->ROWS = rows; this->COLUMNS = columns; this->SIZE = rows*columns; this->MEM_SIZE = sizeof(TYPE)*rows*columns; } memset(Parent::_c, 0, Parent::MEM_SIZE); return *this; } Matrix& apply(std::function fn) { Parent::apply(fn); return *this; } Matrix t() const { Matrix res(this->COLUMNS, this->ROWS); for (size_t row(0); rowROWS; ++row) for (size_t column(0); columnCOLUMNS; ++column) res(column, row) = this->at(row, column); return res; } Matrix i() const { Matrix res(this->ROWS, this->COLUMNS); for (size_t row(0); rowROWS&&rowCOLUMNS; ++row) res(row, row) = 1; return res; } Matrix& inv() { /// calculate using gauss-jordan algorithmus /// @see http://www.mathebibel.de/inverse-matrix-berechnen-nach-gauss-jordan Matrix o(*this); // left side *this = i(); // right side /// 1. lower left part for (size_t column(0); columnCOLUMNS; ++column) { if (columnROWS) { /// 2. normalize pivot to one TYPE pivot(o(column, column)); if (pivot!=1) { o(column, column) = 1; for (size_t pos(column+1); posCOLUMNS; ++pos) o(column, pos)/=pivot; for (size_t pos(0); posCOLUMNS; ++pos) this->at(column, pos)/=pivot; } /// 3. nullify lower triangle for (size_t row(column+1); rowROWS; ++row) { TYPE pivot(o(row, column)); if (pivot!=0) { o(row, column) = 0; for (size_t pos(column+1); posCOLUMNS; ++pos) o(row, pos) -= pivot*o(column, pos); for (size_t pos(0); posCOLUMNS; ++pos) this->at(row, pos) -= pivot*this->at(column, pos); } } } } /// 4. nullify the upper triangle const size_t LASTCOL(this->COLUMNS-1); const size_t LASTROW(this->ROWS-1); for (size_t column(1); columnCOLUMNS; ++column) { for (size_t row(0); rowCOLUMNS; ++pos) o(row, pos) -= pivot*o(column, pos); for (size_t pos(0); posCOLUMNS; ++pos) this->at(row, pos) -= pivot*this->at(column, pos); } } } return *this; } ///@} //................................................................methods protected: virtual void assert_check(const Matrix& o) const { assert(o.ROWS==this->ROWS); assert(o.COLUMNS==this->COLUMNS); } virtual bool check(const Matrix& o) const { return o.ROWS==this->ROWS && o.COLUMNS==this->COLUMNS; } void copy_args(TYPE*) {} template void copy_args(TYPE* to, TYPE t1, ARGS...t) { *to = t1; copy_args(++to, t...); } }; //============================================================================== template Matrix operator+(const Matrix& a, const Matrix& b) { Matrix res(a); res += b; return res; } template Matrix operator-(const Matrix& a, const Matrix& b) { Matrix res(a); res -= b; return res; } template Matrix operator*(const TYPE& v, const Matrix& m) { Matrix res(m); res *= v; return res; } template Matrix operator*(const Matrix& m, const TYPE& v) { Matrix res(m); res *= v; return res; } template Matrix operator/(const Matrix& m, const TYPE& v) { Matrix res(m); res /= v; return res; } template Matrix operator/(const TYPE& v, const Matrix& m) { Matrix res(m); res.inv() *= v; return res; } template Matrix operator/(const Matrix& m1, const Matrix& m2) { Matrix res(m2); return m1 * res.inv(); } template std::ostream& operator<<(std::ostream& s, const Matrix& m) { s<<'['< std::istream& operator>>(std::istream& in, Matrix& m) { std::ios_base::failure err("illegal matrix format"); char c(0); size_t sz(0); TYPE val(0); std::string s; if (!in.get(c) || c!='[') throw err; if (!std::getline(in, s, 'x') || !(std::stringstream(s)>>sz) || sz!=m.rows()) throw err; if (!std::getline(in, s, ']') || !(std::stringstream(s)>>sz) || sz!=m.columns()) throw err; if (!in.get(c) || c!='{') throw err; for (size_t row = 0; row < m.rows(); ++row) { for (size_t column = 0; column < m.columns(); ++column) { if (row==m.rows()-1&&column==m.columns()-1) { if (!std::getline(in, s, '}') || !(std::stringstream(s)>>val)) throw err; } else { if (!std::getline(in, s, ',') || !(std::stringstream(s)>>val)) throw err; } m(row, column) = val; } } return in; } template std::istream& operator>>(std::istream& in, Matrix& m) { std::ios_base::failure err("illegal matrix format"); char c(0); size_t rows(0), columns(0); TYPE val(0); std::string s; if (!in.get(c) || c!='[') throw err; if (!std::getline(in, s, 'x') || !(std::stringstream(s)>>rows) || rows<=0) throw err; if (!std::getline(in, s, ']') || !(std::stringstream(s)>>columns) || columns<=0) throw err; m.resize(rows, columns); if (!in.get(c) || c!='{') throw err; for (size_t row = 0; row < m.rows(); ++row) { for (size_t column = 0; column < m.columns(); ++column) { if (row==m.rows()-1&&column==m.columns()-1) { if (!std::getline(in, s, '}') || !(std::stringstream(s)>>val)) throw err; } else { if (!std::getline(in, s, ',') || !(std::stringstream(s)>>val)) throw err; } m(row, column) = val; } } return in; }