C++ matrix template classe for mathematics.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

802 lines
23 KiB

/*! @file
@id $Id$
*/
// 1 2 3 4 5 6 7 8
// 45678901234567890123456789012345678901234567890123456789012345678901234567890
#include <iostream>
#include <sstream>
#include <cstring>
#include <cassert>
#include <type_traits>
8 years ago
#include <limits>
#include <cmath>
#include <cfenv>
#include <stdexcept>
#include <functional>
8 years ago
8 years ago
/** @mainpage @description
@readme
*/
/// Auxiliary Mathematical Functions
8 years ago
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. */
8 years ago
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. */
8 years ago
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. */
8 years ago
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 {
8 years ago
//..............................................................variables
protected:
size_t ROWS;
size_t COLUMNS;
size_t SIZE;
size_t MEM_SIZE;
//...............................................................typedefs
8 years ago
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;
};
8 years ago
///@}
//................................................................methods
public:
/// @name construction
///@{
8 years ago
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
///@{
8 years ago
/// 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);
}
8 years ago
/// 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);
}
8 years ago
/// Access Matrix Element at Given Row and Column
/** @copydoc at */
TYPE& operator()(size_t row, size_t column) {
return at(row, column);
}
8 years ago
/// Access Matrix Element at Given Row and Column
/** @copydoc at */
const TYPE& operator()(size_t row, size_t column) const {
return at(row, column);
}
8 years ago
/// 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);
}
8 years ago
/// 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);
}
8 years ago
/// Get Number Of Rows
size_t rows() const {
return ROWS;
}
/// Get Number Of Columns
size_t columns() const {
return COLUMNS;
}
///@}
/// @name operators
///@{
8 years ago
/// 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);
8 years ago
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
8 years ago
MatrixBase& operator*=(const TYPE& o) {
TYPE *res((TYPE*)(_c)+SIZE);
while (res>(TYPE*)(_c)) *--res *= o;
return *this;
}
/// Divide Matrix By Scalar
8 years ago
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;
8 years ago
}
/// 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:
8 years ago
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
///@{
8 years ago
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
///@{
8 years ago
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;
}
8 years ago
Matrix operator-() const {
Matrix res(*this);
for (TYPE *to((TYPE*)(res._c)+this->SIZE); to>(TYPE*)(res._c); *--to = -*to);
return res;
}
8 years ago
template<size_t NEWCOLUMNS>
Matrix<TYPE, TROWS, NEWCOLUMNS>
operator*(const Matrix<TYPE, TCOLUMNS, NEWCOLUMNS>& o) const {
8 years ago
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);
8 years ago
return res;
}
///@}
8 years ago
/// @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
8 years ago
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) {
8 years ago
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;
8 years ago
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);
}
}
}
8 years ago
}
/// 4. nullify the upper triangle
8 years ago
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:
8 years ago
/// @name construction
///@{
Matrix() = delete;
8 years ago
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);
}
8 years ago
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;
}
8 years ago
///@}
8 years ago
/// @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
///@{
8 years ago
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;
}
8 years ago
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;
}