|
|
|
/*! @file
|
|
|
|
|
|
|
|
@id $Id$
|
|
|
|
*/
|
|
|
|
// 1 2 3 4 5 6 7 8
|
|
|
|
// 45678901234567890123456789012345678901234567890123456789012345678901234567890
|
|
|
|
|
|
|
|
#include <iostream>
|
|
|
|
#include <sstream>
|
|
|
|
#include <cstring>
|
|
|
|
#include <cassert>
|
|
|
|
#include <type_traits>
|
|
|
|
#include <limits>
|
|
|
|
#include <cmath>
|
|
|
|
#include <cfenv>
|
|
|
|
#include <stdexcept>
|
|
|
|
#include <functional>
|
|
|
|
|
|
|
|
/** @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<typename TYPE>
|
|
|
|
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<TYPE>::epsilon();
|
|
|
|
return diff<=max*1000*std::numeric_limits<TYPE>::epsilon();
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Check Two Values For Equality
|
|
|
|
/** If the values are floating point variables, it calls
|
|
|
|
math::aux::almostEqual. */
|
|
|
|
template<typename TYPE>
|
|
|
|
bool equal(const TYPE& a, const TYPE& b) {
|
|
|
|
return a==b;
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Check if Two <code>long double</code> 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<TYPE,0,0>. Implements generic common methods. */
|
|
|
|
template<typename TYPE, typename ARRAY=TYPE*> 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<int,4,4> 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<typename ...ARGS>
|
|
|
|
MatrixBase(size_t rows, size_t columns, ARGS...t):
|
|
|
|
ROWS(rows), COLUMNS(columns),
|
|
|
|
SIZE(ROWS*COLUMNS), MEM_SIZE(SIZE*sizeof(TYPE)),
|
|
|
|
_c{std::forward<TYPE>(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<int,3,3> 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<ROWS);
|
|
|
|
assert(column<COLUMNS);
|
|
|
|
return *((TYPE*)_c+row*COLUMNS+column);
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Access Matrix Element at Given Row and Column
|
|
|
|
/** @copydoc at */
|
|
|
|
const TYPE& at(size_t row, size_t column) const {
|
|
|
|
assert(row<ROWS);
|
|
|
|
assert(column<COLUMNS);
|
|
|
|
return *((TYPE*)_c+row*COLUMNS+column);
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Access Matrix Element at Given Row and Column
|
|
|
|
/** @copydoc at */
|
|
|
|
TYPE& operator()(size_t row, size_t column) {
|
|
|
|
return at(row, column);
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Access Matrix Element at Given Row and Column
|
|
|
|
/** @copydoc at */
|
|
|
|
const TYPE& operator()(size_t row, size_t column) const {
|
|
|
|
return at(row, column);
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Access Matrix Element at Given Row and Column
|
|
|
|
/** @copydoc at */
|
|
|
|
RowVector operator[](size_t row) {
|
|
|
|
assert(row<ROWS);
|
|
|
|
return RowVector(*this, (TYPE*)_c+row*COLUMNS);
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Access Matrix Element at Given Row and Column
|
|
|
|
/** @copydoc at */
|
|
|
|
const ConstRowVector operator[](size_t row) const {
|
|
|
|
assert(row<ROWS);
|
|
|
|
return ConstRowVector(*this, (TYPE*)_c+row*COLUMNS);
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Get Number Of Rows
|
|
|
|
size_t rows() const {
|
|
|
|
return ROWS;
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Get Number Of Columns
|
|
|
|
size_t columns() const {
|
|
|
|
return COLUMNS;
|
|
|
|
}
|
|
|
|
|
|
|
|
///@}
|
|
|
|
|
|
|
|
/// @name operators
|
|
|
|
///@{
|
|
|
|
|
|
|
|
/// Assign Other Matrix Of Same Size
|
|
|
|
MatrixBase& operator=(const MatrixBase& o) {
|
|
|
|
assert_check(o);
|
|
|
|
memcpy(_c, o._c, MEM_SIZE);
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
|
|
|
|
/// Compare To Other Matrix
|
|
|
|
bool operator==(const MatrixBase& o) const {
|
|
|
|
if (!check(o)) return false;
|
|
|
|
TYPE *to((TYPE*)(_c)+SIZE), *from((TYPE*)(o._c)+SIZE);
|
|
|
|
while (to>(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<void(TYPE&)> 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<COLUMNS-1; ++column) {
|
|
|
|
for (size_t row(column+1); row<ROWS; ++row) {
|
|
|
|
TYPE pivot(at(row, column));
|
|
|
|
if (pivot!=0) {
|
|
|
|
at(row, column) = 0;
|
|
|
|
for (size_t pos(column+1); pos<COLUMNS; ++pos)
|
|
|
|
at(row, pos) -= pivot*at(0, pos);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return lambda;
|
|
|
|
}
|
|
|
|
|
|
|
|
//................................................................methods
|
|
|
|
protected:
|
|
|
|
|
|
|
|
virtual void assert_check(const MatrixBase& o) const {}
|
|
|
|
virtual bool check(const MatrixBase& o) const {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
//..............................................................variables
|
|
|
|
protected:
|
|
|
|
|
|
|
|
ARRAY _c;
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
//==============================================================================
|
|
|
|
|
|
|
|
template<typename TYPE, size_t TROWS=0, size_t TCOLUMNS=0> class Matrix:
|
|
|
|
public MatrixBase<TYPE, TYPE[TROWS][TCOLUMNS]> {
|
|
|
|
|
|
|
|
//...............................................................typedefs
|
|
|
|
private:
|
|
|
|
|
|
|
|
typedef MatrixBase<TYPE, TYPE[TROWS][TCOLUMNS]> 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<typename ...ARGS>
|
|
|
|
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<size_t NEWCOLUMNS>
|
|
|
|
Matrix<TYPE, TROWS, NEWCOLUMNS>
|
|
|
|
operator*(const Matrix<TYPE, TCOLUMNS, NEWCOLUMNS>& o) const {
|
|
|
|
Matrix<TYPE, TROWS, NEWCOLUMNS> res;
|
|
|
|
for (size_t i(0); i<TROWS; ++i)
|
|
|
|
for (size_t k(0); k<NEWCOLUMNS; ++k)
|
|
|
|
for (size_t j(0); j<TCOLUMNS; ++j)
|
|
|
|
res(i, k) += this->at(i, j) * o(j, k);
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
///@}
|
|
|
|
|
|
|
|
/// @name special operations
|
|
|
|
///@{
|
|
|
|
|
|
|
|
Matrix& apply(std::function<void(TYPE&)> fn) {
|
|
|
|
Parent::apply(fn);
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
|
|
|
|
Matrix<TYPE, TCOLUMNS, TROWS> t() const {
|
|
|
|
Matrix<TYPE, TCOLUMNS, TROWS> res;
|
|
|
|
for (size_t row(0); row<TROWS; ++row)
|
|
|
|
for (size_t column(0); column<TCOLUMNS; ++column)
|
|
|
|
res(column, row) = this->at(row, column);
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
static Matrix i() {
|
|
|
|
Matrix res;
|
|
|
|
for (size_t row(0); row<TROWS&&row<TCOLUMNS; ++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); column<this->COLUMNS; ++column) {
|
|
|
|
if (column<this->ROWS) {
|
|
|
|
/// 2. normalize pivot to one
|
|
|
|
TYPE pivot(o(column, column));
|
|
|
|
if (pivot!=1) {
|
|
|
|
o(column, column) = 1;
|
|
|
|
for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
|
|
|
|
o(column, pos)/=pivot;
|
|
|
|
for (size_t pos(0); pos<this->COLUMNS; ++pos)
|
|
|
|
this->at(column, pos)/=pivot;
|
|
|
|
}
|
|
|
|
/// 3. nullify lower triangle
|
|
|
|
for (size_t row(column+1); row<this->ROWS; ++row) {
|
|
|
|
TYPE pivot(o(row, column));
|
|
|
|
if (pivot!=0) {
|
|
|
|
o(row, column) = 0;
|
|
|
|
for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
|
|
|
|
o(row, pos) -= pivot*o(column, pos);
|
|
|
|
for (size_t pos(0); pos<this->COLUMNS; ++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); column<this->COLUMNS; ++column) {
|
|
|
|
for (size_t row(0); row<column && row<LASTROW; ++row) {
|
|
|
|
TYPE pivot(o(row, column));
|
|
|
|
if (pivot!=0) {
|
|
|
|
o(row, column) = 0;
|
|
|
|
for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
|
|
|
|
o(row, pos) -= pivot*o(column, pos);
|
|
|
|
for (size_t pos(0); pos<this->COLUMNS; ++pos)
|
|
|
|
this->at(row, pos) -= pivot*this->at(column, pos);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
|
|
|
|
///@}
|
|
|
|
};
|
|
|
|
|
|
|
|
//==============================================================================
|
|
|
|
|
|
|
|
template<typename TYPE> class Matrix<TYPE, 0, 0>: public MatrixBase<TYPE> {
|
|
|
|
|
|
|
|
//...............................................................typedefs
|
|
|
|
private:
|
|
|
|
|
|
|
|
typedef MatrixBase<TYPE> 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<typename ...ARGS>
|
|
|
|
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<TYPE> res(this->ROWS, o.COLUMNS);
|
|
|
|
assert(this->COLUMNS==o.ROWS);
|
|
|
|
for (size_t i(0); i<this->ROWS; ++i)
|
|
|
|
for (size_t k(0); k<o.COLUMNS; ++k)
|
|
|
|
for (size_t j(0); j<this->COLUMNS; ++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<void(TYPE&)> fn) {
|
|
|
|
Parent::apply(fn);
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
|
|
|
|
Matrix t() const {
|
|
|
|
Matrix res(this->COLUMNS, this->ROWS);
|
|
|
|
for (size_t row(0); row<this->ROWS; ++row)
|
|
|
|
for (size_t column(0); column<this->COLUMNS; ++column)
|
|
|
|
res(column, row) = this->at(row, column);
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
Matrix i() const {
|
|
|
|
Matrix res(this->ROWS, this->COLUMNS);
|
|
|
|
for (size_t row(0); row<this->ROWS&&row<this->COLUMNS; ++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); column<this->COLUMNS; ++column) {
|
|
|
|
if (column<this->ROWS) {
|
|
|
|
/// 2. normalize pivot to one
|
|
|
|
TYPE pivot(o(column, column));
|
|
|
|
if (pivot!=1) {
|
|
|
|
o(column, column) = 1;
|
|
|
|
for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
|
|
|
|
o(column, pos)/=pivot;
|
|
|
|
for (size_t pos(0); pos<this->COLUMNS; ++pos)
|
|
|
|
this->at(column, pos)/=pivot;
|
|
|
|
}
|
|
|
|
/// 3. nullify lower triangle
|
|
|
|
for (size_t row(column+1); row<this->ROWS; ++row) {
|
|
|
|
TYPE pivot(o(row, column));
|
|
|
|
if (pivot!=0) {
|
|
|
|
o(row, column) = 0;
|
|
|
|
for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
|
|
|
|
o(row, pos) -= pivot*o(column, pos);
|
|
|
|
for (size_t pos(0); pos<this->COLUMNS; ++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); column<this->COLUMNS; ++column) {
|
|
|
|
for (size_t row(0); row<column && row<LASTROW; ++row) {
|
|
|
|
TYPE pivot(o(row, column));
|
|
|
|
if (pivot!=0) {
|
|
|
|
o(row, column) = 0;
|
|
|
|
for (size_t pos(column+1); pos<this->COLUMNS; ++pos)
|
|
|
|
o(row, pos) -= pivot*o(column, pos);
|
|
|
|
for (size_t pos(0); pos<this->COLUMNS; ++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<typename ...ARGS>
|
|
|
|
void copy_args(TYPE* to, TYPE t1, ARGS...t) {
|
|
|
|
*to = t1;
|
|
|
|
copy_args(++to, t...);
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
//==============================================================================
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> operator+(const Matrix<TYPE, ROWS, COLUMNS>& a,
|
|
|
|
const Matrix<TYPE, ROWS, COLUMNS>& b) {
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> res(a);
|
|
|
|
res += b;
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> operator-(const Matrix<TYPE, ROWS, COLUMNS>& a,
|
|
|
|
const Matrix<TYPE, ROWS, COLUMNS>& b) {
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> res(a);
|
|
|
|
res -= b;
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> operator*(const TYPE& v,
|
|
|
|
const Matrix<TYPE, ROWS, COLUMNS>& m) {
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> res(m);
|
|
|
|
res *= v;
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> operator*(const Matrix<TYPE, ROWS, COLUMNS>& m,
|
|
|
|
const TYPE& v) {
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> res(m);
|
|
|
|
res *= v;
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> operator/(const Matrix<TYPE, ROWS, COLUMNS>& m,
|
|
|
|
const TYPE& v) {
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> res(m);
|
|
|
|
res /= v;
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> operator/(const TYPE& v,
|
|
|
|
const Matrix<TYPE, ROWS, COLUMNS>& m) {
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> res(m);
|
|
|
|
res.inv() *= v;
|
|
|
|
return res;
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> operator/(const Matrix<TYPE, ROWS, COLUMNS>& m1,
|
|
|
|
const Matrix<TYPE, ROWS, COLUMNS>& m2) {
|
|
|
|
Matrix<TYPE, ROWS, COLUMNS> res(m2);
|
|
|
|
return m1 * res.inv();
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
std::ostream& operator<<(std::ostream& s, const Matrix<TYPE, ROWS, COLUMNS>& m) {
|
|
|
|
s<<'['<<m.rows()<<'x'<<m.columns()<<"]{";
|
|
|
|
for (size_t row = 0; row < m.rows(); ++row) {
|
|
|
|
for (size_t column = 0; column < m.columns(); ++column) {
|
|
|
|
if (row!=0||column!=0) s<<',';
|
|
|
|
s<<m(row, column);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return s<<'}';
|
|
|
|
}
|
|
|
|
|
|
|
|
template<typename TYPE, size_t ROWS, size_t COLUMNS>
|
|
|
|
std::istream& operator>>(std::istream& in, Matrix<TYPE, ROWS, COLUMNS>& 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<typename TYPE>
|
|
|
|
std::istream& operator>>(std::istream& in, Matrix<TYPE, 0, 0>& 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;
|
|
|
|
}
|