1369 lines
27 KiB
C++
1369 lines
27 KiB
C++
#pragma once
|
|
|
|
#include "vec.h"
|
|
#include <limits>
|
|
#include <cassert>
|
|
|
|
#ifdef _MSC_VER
|
|
#pragma warning(disable : 4996) //checked iterators
|
|
#endif
|
|
|
|
namespace cgv {
|
|
/// namespace with classes and algorithms for mathematics
|
|
namespace math {
|
|
|
|
|
|
|
|
template <typename T>
|
|
class mat;
|
|
|
|
|
|
|
|
|
|
template <typename RandomAccessIterator>
|
|
struct step_iterator:
|
|
public std::iterator<typename std::random_access_iterator_tag,typename std::iterator_traits<RandomAccessIterator>::value_type>
|
|
|
|
{
|
|
public:
|
|
typedef std::iterator<typename std::random_access_iterator_tag,typename std::iterator_traits<RandomAccessIterator>::value_type> base_type;
|
|
typedef typename base_type::pointer pointer;
|
|
typedef typename base_type::reference reference;
|
|
typedef typename base_type::value_type value_type;
|
|
typedef typename base_type::difference_type difference_type;
|
|
private:
|
|
RandomAccessIterator internal_iter;
|
|
int step;
|
|
|
|
|
|
friend class mat<value_type>;
|
|
|
|
step_iterator( RandomAccessIterator begin,int step=1):internal_iter(begin),step(step)
|
|
{}
|
|
|
|
|
|
public:
|
|
|
|
|
|
step_iterator():internal_iter(NULL)
|
|
{
|
|
step=0;
|
|
}
|
|
|
|
step_iterator(const step_iterator& other)
|
|
{
|
|
internal_iter=other.internal_iter;
|
|
step=other.step;
|
|
}
|
|
|
|
step_iterator& operator=(const step_iterator& other)
|
|
{
|
|
if(*this != other)
|
|
{
|
|
internal_iter = other.internal_iter;
|
|
step = other.step;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
bool operator==(const step_iterator& other) const
|
|
{
|
|
return internal_iter == other.internal_iter;
|
|
}
|
|
|
|
bool operator!=(const step_iterator& other) const
|
|
{
|
|
return !(*this==other);
|
|
}
|
|
|
|
reference operator*()
|
|
{
|
|
return *internal_iter;
|
|
}
|
|
|
|
reference operator*() const
|
|
{
|
|
return *internal_iter;
|
|
}
|
|
pointer operator->()
|
|
{
|
|
return &**this;
|
|
}
|
|
|
|
pointer operator->() const
|
|
{
|
|
return &**this;
|
|
}
|
|
|
|
step_iterator& operator ++()
|
|
{
|
|
|
|
internal_iter+=step;
|
|
return *this;
|
|
}
|
|
|
|
step_iterator operator ++(int)
|
|
{
|
|
step_iterator tmp=*this;
|
|
++*this;
|
|
return tmp;
|
|
|
|
}
|
|
step_iterator& operator --()
|
|
{
|
|
internal_iter-=step;
|
|
return *this;
|
|
}
|
|
step_iterator operator --(int)
|
|
{
|
|
step_iterator tmp=*this;
|
|
--*this;
|
|
return tmp;
|
|
}
|
|
|
|
|
|
|
|
step_iterator& operator +=(difference_type n)
|
|
{
|
|
internal_iter+=n*step;
|
|
return *this;
|
|
|
|
}
|
|
|
|
step_iterator& operator -=(difference_type n)
|
|
{
|
|
internal_iter-=n*step;
|
|
return *this;
|
|
}
|
|
|
|
step_iterator operator -(difference_type n) const
|
|
{
|
|
step_iterator tmp=*this;
|
|
tmp-=n;
|
|
return tmp;
|
|
}
|
|
difference_type operator-(const step_iterator& right) const
|
|
{
|
|
return (internal_iter - right.internal_iter)/step;
|
|
}
|
|
|
|
|
|
step_iterator operator +(difference_type n)const
|
|
{
|
|
step_iterator tmp=*this;
|
|
tmp+=n;
|
|
return tmp;
|
|
}
|
|
|
|
reference operator[](difference_type offset) const
|
|
{
|
|
return (*(*this + offset));
|
|
}
|
|
|
|
bool operator <(const step_iterator& other) const
|
|
{
|
|
if(step > 0)
|
|
return internal_iter < other.internal_iter;
|
|
else
|
|
return internal_iter > other.internal_iter;
|
|
}
|
|
|
|
bool operator >(const step_iterator& other) const
|
|
{
|
|
if(step > 0)
|
|
return internal_iter > other.internal_iter;
|
|
else
|
|
return internal_iter < other.internal_iter;
|
|
}
|
|
|
|
bool operator <=(const step_iterator& other) const
|
|
{
|
|
if(step > 0)
|
|
return internal_iter <= other.internal_iter;
|
|
else
|
|
return internal_iter >= other.internal_iter;
|
|
}
|
|
|
|
bool operator >=(const step_iterator& other) const
|
|
{
|
|
if(step > 0)
|
|
return internal_iter >= other.internal_iter;
|
|
else
|
|
return internal_iter <= other.internal_iter;
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
* A matrix type (full column major storage)
|
|
* The matrix can be loaded directly into OpenGL without need for transposing!
|
|
*/
|
|
template <typename T>
|
|
class mat
|
|
{
|
|
protected:
|
|
///pointer to data storage
|
|
vec<T> _data;
|
|
///number of columns
|
|
unsigned _ncols;
|
|
///number of rows
|
|
unsigned _nrows;
|
|
|
|
|
|
public:
|
|
typedef typename vec<T>::value_type value_type;
|
|
typedef typename vec<T>::reference reference;
|
|
typedef typename vec<T>::const_reference const_reference;
|
|
typedef typename vec<T>::pointer pointer;
|
|
typedef typename vec<T>::const_pointer const_pointer;
|
|
typedef typename vec<T>::iterator iterator;
|
|
typedef typename vec<T>::const_iterator const_iterator;
|
|
typedef typename vec<T>::reverse_iterator reverse_iterator;
|
|
typedef typename vec<T>::const_reverse_iterator const_reverse_iterator;
|
|
|
|
typedef iterator col_iterator;
|
|
typedef const col_iterator const_col_iterator;
|
|
typedef std::reverse_iterator<col_iterator> reverse_col_iterator;
|
|
typedef std::reverse_iterator<const_col_iterator> const_reverse_col_iterator;
|
|
|
|
typedef step_iterator<iterator> row_iterator;
|
|
typedef const row_iterator const_row_iterator;
|
|
typedef std::reverse_iterator<row_iterator> reverse_row_iterator;
|
|
typedef std::reverse_iterator<const_row_iterator> const_reverse_row_iterator;
|
|
|
|
typedef step_iterator<iterator > diag_iterator;
|
|
typedef const diag_iterator const_diag_iterator;
|
|
typedef std::reverse_iterator<diag_iterator> reverse_diag_iterator;
|
|
typedef std::reverse_iterator<const_diag_iterator> const_reverse_diag_iterator;
|
|
|
|
typedef step_iterator<iterator > anti_diag_iterator;
|
|
typedef const anti_diag_iterator const_anti_diag_iterator;
|
|
typedef std::reverse_iterator<anti_diag_iterator> reverse_anti_diag_iterator;
|
|
typedef std::reverse_iterator<const_anti_diag_iterator> const_reverse_anti_diag_iterator;
|
|
|
|
|
|
|
|
|
|
|
|
iterator begin(){return _data.begin();}
|
|
iterator end(){return _data.end();}
|
|
const_iterator begin() const{return _data.begin();}
|
|
const_iterator end() const{return _data.end();}
|
|
reverse_iterator rbegin(){return _data.rbegin();}
|
|
reverse_iterator rend(){return _data.rend();}
|
|
const_reverse_iterator rbegin() const{return _data.rbegin();}
|
|
const_reverse_iterator rend() const {return _data.rend();}
|
|
|
|
|
|
|
|
col_iterator col_begin(int c)
|
|
{
|
|
return col_iterator(_data.begin()+(c*_nrows));
|
|
}
|
|
col_iterator col_end(int c)
|
|
{
|
|
return col_iterator(_data.begin()+(c+1)*_nrows );
|
|
}
|
|
const_col_iterator col_begin(int c) const
|
|
{
|
|
return const_col_iterator(_data.begin()+(c*_nrows));
|
|
}
|
|
const_col_iterator col_end(int c) const
|
|
{
|
|
return const_col_iterator(_data.begin()+(c+1)*_nrows );
|
|
}
|
|
reverse_col_iterator col_rbegin(int c)
|
|
{
|
|
return (reverse_col_iterator(col_end(c)));
|
|
}
|
|
reverse_col_iterator col_rend(int c)
|
|
{
|
|
return (reverse_col_iterator(col_begin(c)));
|
|
}
|
|
const_reverse_col_iterator col_rbegin(int c) const
|
|
{
|
|
return (const_reverse_col_iterator(col_end(c)));
|
|
}
|
|
const_reverse_col_iterator col_rend(int c) const
|
|
{
|
|
return (const_reverse_col_iterator(col_begin(c)));
|
|
}
|
|
|
|
row_iterator row_begin(int r)
|
|
{
|
|
return row_iterator(_data.begin()+r,_nrows);
|
|
}
|
|
row_iterator row_end(int r)
|
|
{
|
|
return row_iterator(_data.begin()+(r+_ncols*_nrows),_nrows);
|
|
}
|
|
const_row_iterator row_begin(int r) const
|
|
{
|
|
return const_row_iterator(_data.begin()+r,_nrows);
|
|
}
|
|
const_row_iterator row_end(int r) const
|
|
{
|
|
return const_row_iterator(_data.begin()+(r+_ncols*_nrows),_nrows);
|
|
}
|
|
|
|
reverse_row_iterator row_rbegin(int r)
|
|
{
|
|
return (reverse_row_iterator(row_end(r)));
|
|
}
|
|
|
|
reverse_row_iterator row_rend(int r)
|
|
{
|
|
return (reverse_row_iterator(row_begin(r)));
|
|
}
|
|
|
|
const_reverse_row_iterator row_rbegin(int r) const
|
|
{
|
|
return (const_reverse_row_iterator(row_end(r)));
|
|
}
|
|
|
|
const_reverse_row_iterator row_rend(int r) const
|
|
{
|
|
return (const_reverse_row_iterator(row_begin(r)));
|
|
}
|
|
|
|
|
|
|
|
|
|
diag_iterator diag_begin(int d=0)
|
|
{
|
|
if(d <= 0)
|
|
return diag_iterator(_data.begin()-d ,_nrows+1);
|
|
else
|
|
return diag_iterator(_data.begin()+d*_nrows,_nrows+1);
|
|
|
|
}
|
|
diag_iterator diag_end(int d=0)
|
|
{
|
|
if(d <= 0)
|
|
{
|
|
int n=std::min<int>(_nrows+d,_ncols);
|
|
return diag_iterator(_data.begin()-d+n*(_nrows+1),_nrows+1);
|
|
}
|
|
else
|
|
{
|
|
int n = std::min<int>(_ncols-d,_nrows);
|
|
return diag_iterator(_data.begin()+d*_nrows+n*(_nrows+1),_nrows+1) ;
|
|
}
|
|
}
|
|
const_diag_iterator diag_begin(int d=0) const
|
|
{
|
|
if(d <= 0)
|
|
return const_diag_iterator(begin()-d ,_nrows+1);
|
|
else
|
|
return const_diag_iterator(begin()+d*_nrows,_nrows+1);
|
|
|
|
}
|
|
const_diag_iterator diag_end(int d=0) const
|
|
{
|
|
if(d <= 0)
|
|
{
|
|
int n=std::min<int>(_nrows+d,_ncols);
|
|
return const_diag_iterator(_data.begin()-d+n*(_nrows+1),_nrows+1);
|
|
}
|
|
else
|
|
{
|
|
int n = std::min<int>(_ncols-d,_nrows);
|
|
return const_diag_iterator(_data.begin()+d*_nrows+n*(_nrows+1),_nrows+1) ;
|
|
}
|
|
}
|
|
reverse_diag_iterator diag_rbegin(int d=0)
|
|
{
|
|
return (reverse_diag_iterator(diag_end(d)));
|
|
}
|
|
reverse_diag_iterator diag_rend(int d=0)
|
|
{
|
|
return (reverse_diag_iterator(diag_begin(d)));
|
|
}
|
|
const_reverse_diag_iterator diag_rbegin(int d=0) const
|
|
{
|
|
return (const_reverse_diag_iterator(diag_end(d)));
|
|
}
|
|
const_reverse_diag_iterator diag_rend(int d=0) const
|
|
{
|
|
return (const_reverse_diag_iterator(diag_begin(d)));
|
|
}
|
|
|
|
anti_diag_iterator anti_diag_begin(int d=0)
|
|
{
|
|
if(d >= 0)
|
|
return anti_diag_iterator(_data.begin()+(_ncols-1-d)*_nrows ,1-_nrows);
|
|
else
|
|
return anti_diag_iterator(_data.begin()+(_ncols-1)*_nrows-d ,1-_nrows);
|
|
}
|
|
|
|
anti_diag_iterator anti_diag_end(int d=0)
|
|
{
|
|
if(d >= 0)
|
|
{
|
|
int n=std::min(_ncols-d,_nrows);
|
|
return anti_diag_iterator(_data.begin()+(_ncols-1-d)*_nrows +n*(1-_nrows),1-_nrows);
|
|
}
|
|
else
|
|
{
|
|
int n=std::min(_nrows+d,_ncols);
|
|
return anti_diag_iterator(_data.begin()+(_ncols-1)*_nrows-d +n*(1-_nrows),1-_nrows);
|
|
}
|
|
}
|
|
|
|
const_anti_diag_iterator anti_diag_begin(int d=0) const
|
|
{
|
|
if(d >= 0)
|
|
return const_anti_diag_iterator(_data.begin()+(_ncols-1-d)*_nrows ,1-_nrows);
|
|
else
|
|
return const_anti_diag_iterator(_data.begin()+(_ncols-1)*_nrows-d ,1-_nrows);
|
|
}
|
|
|
|
const_anti_diag_iterator anti_diag_end(int d=0) const
|
|
{
|
|
if(d >= 0)
|
|
{
|
|
int n=std::min(_ncols-d,_nrows);
|
|
return const_anti_diag_iterator(_data.begin()+(_ncols-1-d)*_nrows +n*(1-_nrows),1-_nrows);
|
|
}
|
|
else
|
|
{
|
|
int n=std::min(_nrows+d,_ncols);
|
|
return const_anti_diag_iterator(_data.begin()+(_ncols-1)*_nrows-d +n*(1-_nrows),1-_nrows);
|
|
}
|
|
}
|
|
|
|
reverse_anti_diag_iterator anti_diag_rbegin(int d=0)
|
|
{
|
|
return (reverse_anti_diag_iterator(anti_diag_end(d)));
|
|
}
|
|
|
|
reverse_anti_diag_iterator anti_diag_rend(int d=0)
|
|
{
|
|
return (reverse_anti_diag_iterator(anti_diag_begin(d)));
|
|
}
|
|
|
|
const_reverse_anti_diag_iterator anti_diag_rbegin(int d=0) const
|
|
{
|
|
return (const_reverse_anti_diag_iterator(anti_diag_end(d)));
|
|
}
|
|
|
|
const_reverse_anti_diag_iterator anti_diag_rend(int d=0) const
|
|
{
|
|
return (const_reverse_anti_diag_iterator(anti_diag_begin(d)));
|
|
}
|
|
|
|
///standard constructor
|
|
mat()
|
|
{
|
|
_ncols = 0;
|
|
_nrows = 0;
|
|
}
|
|
|
|
|
|
///constructor creates a nrows x ncols full matrix
|
|
mat(unsigned nrows, unsigned ncols) : _data(nrows*ncols)
|
|
{
|
|
_nrows = nrows;
|
|
_ncols = ncols;
|
|
}
|
|
|
|
///construct a matrix with all elements set to c
|
|
mat(unsigned nrows, unsigned ncols, const T& c) :_data(nrows*ncols)
|
|
{
|
|
_nrows = nrows;
|
|
_ncols = ncols;
|
|
_data.fill(c);
|
|
}
|
|
|
|
|
|
///creates a matrix from an array
|
|
///if the matrix data is stored in a row major fashion set column_major to false
|
|
mat(unsigned nrows, unsigned ncols, const T* marray,bool column_major=true)
|
|
:_data(nrows*ncols)
|
|
{
|
|
_nrows = nrows;
|
|
_ncols = ncols;
|
|
if(column_major)
|
|
memcpy(_data, marray, size()*sizeof(T));
|
|
else
|
|
{
|
|
|
|
for(unsigned i = 0; i < _nrows; i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
{
|
|
operator()(i,j) = marray[i*_ncols +j];
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
///copy constructor for matrix with different element type
|
|
template <typename S>
|
|
mat(const mat<S>& m):_data(m.size())
|
|
{
|
|
_nrows = m.nrows();
|
|
_ncols = m.ncols();
|
|
for(unsigned i = 0; i < _nrows; i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
{
|
|
operator()(i,j)=(T)m(i,j);
|
|
}
|
|
|
|
}
|
|
|
|
/// set data pointer to an external data array
|
|
void set_extern_data(unsigned nrows, unsigned ncols, T* data)
|
|
{
|
|
_data.set_extern_data(nrows*ncols,data);
|
|
_nrows = nrows;
|
|
_ncols = ncols;
|
|
}
|
|
void destruct()
|
|
{
|
|
_data.destruct();
|
|
}
|
|
///number of stored elements
|
|
unsigned size() const
|
|
{
|
|
return _data.size();
|
|
}
|
|
|
|
///number of rows
|
|
unsigned nrows() const
|
|
{
|
|
return _nrows;
|
|
}
|
|
|
|
///number of columns
|
|
unsigned ncols() const
|
|
{
|
|
return _ncols;
|
|
}
|
|
|
|
|
|
|
|
///assignment of a matrix with a different element type
|
|
template <typename S>
|
|
mat<T>& operator = (const mat<S>& m)
|
|
{
|
|
resize(m.nrows(),m.ncols());
|
|
for(unsigned i = 0; i < _nrows; i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
{
|
|
operator()(i,j)=(T)m(i,j);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
///assignment of a scalar s to each element of the matrix
|
|
mat<T>& operator = (const T& s)
|
|
{
|
|
fill (s);
|
|
return *this;
|
|
}
|
|
|
|
///resize the matrix, the content of the matrix will be destroyed
|
|
void resize(unsigned rows, unsigned cols)
|
|
{
|
|
|
|
unsigned newsize = rows*cols;
|
|
_nrows = rows;
|
|
_ncols = cols;
|
|
_data.resize(newsize);
|
|
}
|
|
|
|
///cast operator for non const array
|
|
operator T*()
|
|
{
|
|
return (T*)_data;
|
|
}
|
|
|
|
///cast operator const array
|
|
operator const T*() const
|
|
{
|
|
return (const T*)_data;
|
|
}
|
|
|
|
///returns true if matrix is a square matrix
|
|
bool is_square() const
|
|
{
|
|
return _ncols == _nrows;
|
|
}
|
|
|
|
///fills all elements of the matrix with v
|
|
template <typename S>
|
|
void fill(const S& v)
|
|
{
|
|
T val = (T)v;
|
|
_data.fill(val);
|
|
}
|
|
|
|
///access to the element in the ith row in column j
|
|
T& operator () (unsigned i, unsigned j)
|
|
{
|
|
assert(i < _nrows && j < _ncols);
|
|
return _data[j*_nrows+i];
|
|
}
|
|
|
|
///const access to the element in the ith row on column j
|
|
const T& operator () (unsigned i, unsigned j) const
|
|
{
|
|
assert(_data != NULL && i < _nrows && j < _ncols);
|
|
return _data[j*_nrows+i];
|
|
}
|
|
|
|
///test for equality
|
|
template <typename S>
|
|
bool operator == (const mat<S>& m) const
|
|
{
|
|
if(_ncols != m.ncols() || _nrows != m.nrows())
|
|
return false;
|
|
|
|
return _data == m._data;
|
|
}
|
|
|
|
///test for inequality
|
|
template <typename S>
|
|
bool operator != (const mat<S>& m) const
|
|
{
|
|
if(_ncols != m.ncols() || _nrows != m.nrows())
|
|
return false;
|
|
return _data != m._data;
|
|
return false;
|
|
}
|
|
|
|
|
|
//in place scalar multiplication
|
|
mat<T>& operator *= (const T& s)
|
|
{
|
|
_data *= (T)s;
|
|
return *this;
|
|
}
|
|
|
|
///scalar multiplication
|
|
const mat<T> operator*(const T& s) const
|
|
{
|
|
mat<T> r=(*this);
|
|
r*=(T)s;
|
|
return r;
|
|
}
|
|
|
|
///in place division by a scalar
|
|
mat<T>& operator /= (const T& s)
|
|
{
|
|
_data /= (T)s;
|
|
return *this;
|
|
}
|
|
|
|
/// division by a scalar
|
|
const mat<T> operator / (const T& s) const
|
|
{
|
|
mat<T> r=(*this);
|
|
r/=s;
|
|
return r;
|
|
}
|
|
|
|
|
|
///in place addition by a scalar
|
|
mat<T>& operator += (const T& s)
|
|
{
|
|
_data += (T)s;
|
|
return *this;
|
|
}
|
|
|
|
///componentwise addition of a scalar
|
|
const mat<T> operator + (const T& s)
|
|
{
|
|
mat<T> r=(*this);
|
|
r+=s;
|
|
return r;
|
|
}
|
|
|
|
///in place substraction of a scalar
|
|
mat<T>& operator -= (const T& s)
|
|
{
|
|
_data -= (T)s;
|
|
return *this;
|
|
}
|
|
|
|
/// componentwise subtraction of a scalar
|
|
const mat<T> operator - (const T& s)
|
|
{
|
|
mat<T> r=(*this);
|
|
r-=s;
|
|
return r;
|
|
}
|
|
|
|
///negation operator
|
|
const mat<T> operator-() const
|
|
{
|
|
mat<T> r=(*this)*((T)-1);
|
|
return r;
|
|
}
|
|
|
|
///in place addition of matrix
|
|
template <typename S>
|
|
mat<T>& operator += (const mat<S>& m)
|
|
{
|
|
assert(_ncols == m.ncols() && _nrows == m.nrows());
|
|
_data+=m._data;
|
|
return *this;
|
|
}
|
|
|
|
///in place subtraction of matrix
|
|
template <typename S>
|
|
mat<T>& operator -= (const mat<S>& m)
|
|
{
|
|
assert(_ncols == m.ncols() && _nrows == m.nrows());
|
|
_data-=m._data;
|
|
return *this;
|
|
}
|
|
|
|
|
|
|
|
|
|
///matrix addition
|
|
template <typename S>
|
|
const mat<T> operator+(const mat<S> m2)const
|
|
{
|
|
mat<T> r=(*this);
|
|
r += m2;
|
|
return r;
|
|
}
|
|
|
|
///matrix subtraction
|
|
template <typename S>
|
|
const mat<T> operator-(const mat<S> m2)const
|
|
{
|
|
mat<T> r=(*this);
|
|
r-= m2;
|
|
return r;
|
|
}
|
|
|
|
|
|
///in place matrix multiplication with a ncols x ncols matrix m2
|
|
template <typename S>
|
|
const mat<T> operator*=(const mat<S>& m2)
|
|
{
|
|
assert(ncols() == m2.ncols() && nrows() == m2.nrows() && ncols() == nrows());
|
|
mat<T> r(_nrows,_ncols,(T)0);
|
|
|
|
for(unsigned i = 0; i < _nrows; i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
for(unsigned k = 0; k < _ncols; k++)
|
|
r(i,j) += operator()(i,k) * (T)(m2(k,j));
|
|
(*this)=r;
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
|
|
///multiplication with a ncols x M matrix m2
|
|
template <typename S>
|
|
const mat<T> operator*(const mat<S>& m2) const
|
|
{
|
|
assert(m2.nrows() == _ncols);
|
|
unsigned M = m2.ncols();
|
|
mat<T> r(_nrows,M,(T)0);
|
|
for(unsigned i = 0; i < _nrows; i++)
|
|
for(unsigned j = 0; j < M;j++)
|
|
for(unsigned k = 0; k < _ncols; k++)
|
|
r(i,j) += operator()(i,k) * (T)(m2(k,j));
|
|
|
|
return r;
|
|
}
|
|
|
|
|
|
///matrix vector multiplication
|
|
template < typename S>
|
|
const vec<T> operator*(const vec<S>& v) const
|
|
{
|
|
assert(_ncols==v.size());
|
|
vec<T> r;
|
|
r.zeros(_nrows);
|
|
|
|
for(unsigned j = 0; j < _ncols; j++)
|
|
for(unsigned i = 0; i < _nrows; i++)
|
|
r(i) += operator()(i,j) * (T)(v(j));
|
|
|
|
return r;
|
|
}
|
|
|
|
///create submatrix m(top,left)...m(top+rows,left+cols)
|
|
mat<T> sub_mat(unsigned top, unsigned left, unsigned rows, unsigned cols) const
|
|
{
|
|
|
|
mat<T> mnew(rows,cols);
|
|
|
|
for(unsigned i = 0; i < rows;i++)
|
|
for(unsigned j = 0; j < cols;j++)
|
|
mnew(i,j)=operator()(i+top,j+left);
|
|
|
|
return mnew;
|
|
}
|
|
|
|
///extract a row from the matrix as a vector
|
|
const vec<T> row(unsigned i) const
|
|
{
|
|
|
|
vec<T> mnew(_ncols);
|
|
|
|
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
mnew(j)=operator()(i,j);
|
|
|
|
return mnew;
|
|
}
|
|
|
|
///set row i of the matrix to vector v
|
|
void set_row(unsigned i,const vec<T>& v)
|
|
{
|
|
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
operator()(i,j)=v(j);
|
|
|
|
|
|
}
|
|
|
|
///extract a column of the matrix as a vector
|
|
const vec<T> col(unsigned j) const
|
|
{
|
|
|
|
vec<T> mnew(_nrows);
|
|
|
|
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
mnew(i)=operator()(i,j);
|
|
|
|
return mnew;
|
|
}
|
|
|
|
///set column j of the matrix to vector v
|
|
void set_col(unsigned j,const vec<T>& v)
|
|
{
|
|
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
operator()(i,j)=v(i);
|
|
}
|
|
|
|
|
|
|
|
|
|
///copy submatrix m(top,left)...m(top+rows,left+cols) into submat
|
|
void copy(unsigned top, unsigned left, unsigned rows, unsigned cols, mat<T>& submat) const
|
|
{
|
|
assert(submat.nrows() == rows && submat.ncols() == cols);
|
|
|
|
for(unsigned i = 0; i < rows;i++)
|
|
for(unsigned j = 0; j < cols;j++)
|
|
submat(i,j)=operator()(i+top,j+left);
|
|
|
|
|
|
}
|
|
|
|
///paste matrix m at position: top, left
|
|
void paste(int top, int left,const mat<T>& m)
|
|
{
|
|
for(unsigned i = 0; i < m.nrows(); i++)
|
|
for(unsigned j = 0; j < m.ncols(); j++)
|
|
operator()(i+top,j+left)=m(i,j);
|
|
|
|
}
|
|
|
|
|
|
///exchange row i with row j
|
|
void swap_rows(unsigned i, unsigned j)
|
|
{
|
|
assert(i < _nrows && j < _nrows);
|
|
for(unsigned k = 0; k < _ncols;k++)
|
|
std::swap(operator()(i,k),operator()(j,k));
|
|
}
|
|
|
|
///exchange column i with column j
|
|
void swap_columns(unsigned i, unsigned j)
|
|
{
|
|
assert(i < _ncols && j < _ncols);
|
|
for(unsigned k = 0; k < _nrows;k++)
|
|
std::swap(operator()(k,i),operator()(k,j));
|
|
|
|
}
|
|
|
|
///exchange diagonal elements (i,i) (j,j)
|
|
void swap_diagonal_elements(unsigned i, unsigned j)
|
|
{
|
|
assert(i < _ncols && j < _ncols);
|
|
std::swap(operator()(i,i),operator()(j,j));
|
|
}
|
|
|
|
|
|
|
|
|
|
///returns the trace
|
|
T trace() const
|
|
{
|
|
assert(_nrows == _ncols);
|
|
T t = 0;
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
t+=operator()(i,i);
|
|
return t;
|
|
}
|
|
|
|
///transpose matrix
|
|
void transpose()
|
|
{
|
|
mat<T> r(_ncols,_nrows);
|
|
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
r(j,i) = operator()(i,j);
|
|
|
|
*this = r;
|
|
}
|
|
|
|
//flip columns left-right
|
|
void fliplr()
|
|
{
|
|
mat<T> r(_nrows,_ncols);
|
|
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
r(i,j) = operator()(i,_ncols-j-1);
|
|
|
|
*this = r;
|
|
|
|
}
|
|
|
|
//flip rows up-down
|
|
void flipud()
|
|
{
|
|
mat<T> r(_nrows,_ncols);
|
|
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
r(i,j) = operator()(_nrows-i-1,j);
|
|
|
|
*this = r;
|
|
|
|
}
|
|
|
|
///ceil all components of the matrix
|
|
void ceil()
|
|
{
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
operator()(i,j) =::ceil(operator()(i,j));
|
|
}
|
|
|
|
///floor all components of the matrix
|
|
void floor()
|
|
{
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
operator()(i,j) =::floor(operator()(i,j));
|
|
}
|
|
|
|
///round to integer
|
|
void round()
|
|
{
|
|
for(unsigned i = 0; i < _nrows;i++)
|
|
for(unsigned j = 0; j < _ncols;j++)
|
|
operator()(i,j) =::floor(operator()(i,j)+(T)0.5);
|
|
|
|
}
|
|
|
|
|
|
|
|
///returns the frobenius norm of matrix m
|
|
T frobenius_norm() const
|
|
{
|
|
T n=0;
|
|
for(unsigned i =0; i < _nrows;i++)
|
|
for(unsigned j=0;j <_ncols;j++)
|
|
n+=operator()(i,j)*operator()(i,j);
|
|
|
|
return (T)sqrt((double)n);
|
|
}
|
|
|
|
|
|
///set identity matrix
|
|
void identity()
|
|
{
|
|
assert(_ncols == _nrows);
|
|
zeros();
|
|
for(unsigned i = 0; i < _ncols;++i)
|
|
operator()(i,i)=1;
|
|
}
|
|
|
|
///set dim x dim identity matrix
|
|
void identity(unsigned dim)
|
|
{
|
|
zeros(dim,dim);
|
|
for(unsigned i = 0; i < _ncols;++i)
|
|
operator()(i,i)=1;
|
|
}
|
|
|
|
///set zero matrix
|
|
void zeros()
|
|
{
|
|
fill(0);
|
|
}
|
|
|
|
///resize and fill matrix with zeros
|
|
void zeros(unsigned rows, unsigned cols)
|
|
{
|
|
resize(rows,cols);
|
|
fill((T)0);
|
|
}
|
|
|
|
///resize and fill matrix with ones
|
|
void ones(unsigned rows, unsigned cols)
|
|
{
|
|
resize(rows,cols);
|
|
fill((T)1);
|
|
}
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
template <typename T>
|
|
mat<T> zeros(unsigned rows, unsigned cols)
|
|
{
|
|
mat<T> m;
|
|
m.zeros(rows,cols);
|
|
return m;
|
|
}
|
|
|
|
template <typename T>
|
|
mat<T> ones(unsigned rows, unsigned cols)
|
|
{
|
|
mat<T> m(rows,cols);
|
|
m.fill((T)1);
|
|
return m;
|
|
}
|
|
|
|
template<typename T>
|
|
mat<T> identity(unsigned dim)
|
|
{
|
|
mat<T> m;
|
|
m.identity(dim);
|
|
return m;
|
|
}
|
|
|
|
//return frobenius norm
|
|
template<typename T>
|
|
T frobenius_norm(const mat<T>& m)
|
|
{
|
|
return m.frobenius_norm();
|
|
}
|
|
|
|
//return trace of matrix
|
|
template<typename T>
|
|
T trace(const mat<T>& m)
|
|
{
|
|
return m.trace();
|
|
}
|
|
|
|
|
|
//concatenate two matrices horizontally
|
|
template<typename T, typename S>
|
|
const mat<T> horzcat(const mat<T>& m1, const mat<S>& m2)
|
|
{
|
|
|
|
assert(m1.nrows() == m2.nrows());
|
|
mat<T> r(m1.nrows(),m1.ncols()+m2.ncols());
|
|
for(unsigned i = 0; i < m1.nrows();i++)
|
|
{
|
|
for(unsigned j = 0; j < m1.ncols(); j++)
|
|
r(i,j) = m1(i,j);
|
|
for(unsigned j = 0; j < m2.ncols(); j++)
|
|
r(i,j+m1.ncols())=(T)m2(i,j);
|
|
}
|
|
return r;
|
|
}
|
|
|
|
//concatenates a matrix and a column vector horizontally
|
|
template<typename T, typename S>
|
|
const mat<T> horzcat(const mat<T>& m1, const vec<S>& v)
|
|
{
|
|
|
|
assert(m1.nrows() == v.size());
|
|
mat<T> r(m1.nrows(),m1.ncols()+1);
|
|
for(unsigned i = 0; i < m1.nrows();i++)
|
|
{
|
|
for(unsigned j = 0; j < m1.ncols(); j++)
|
|
r(i,j) = m1(i,j);
|
|
|
|
r(i,m1.ncols())=(T)v(i);
|
|
}
|
|
return r;
|
|
}
|
|
|
|
//concatenates two vectors horizontally
|
|
template<typename T, typename S>
|
|
const mat<T> horzcat(const vec<T>& v1, const vec<S>& v2)
|
|
{
|
|
|
|
assert(v1.size() == v2.size());
|
|
mat<T> r(v1.size(),2);
|
|
for(unsigned i = 0; i < v1.size();i++)
|
|
{
|
|
r(i,0) = v1(i);
|
|
r(i,1) = v2(i);
|
|
}
|
|
return r;
|
|
}
|
|
|
|
//concatenates two column vectors vertically
|
|
template<typename T, typename S>
|
|
const vec<T> vertcat(const vec<T>& v1, const vec<S>& v2)
|
|
{
|
|
|
|
vec<T> r(v1.size()+v2.size());
|
|
unsigned off = v1.size();
|
|
for(unsigned i = 0; i < v1.size();i++)
|
|
r(i) = v1(i);
|
|
for(unsigned i = 0; i < v2.size();i++)
|
|
r(i+off) = v2(i);
|
|
|
|
return r;
|
|
}
|
|
|
|
|
|
//concatenates two matrices vertically
|
|
template<typename T, typename S>
|
|
const mat<T> vertcat(const mat<T>& m1, const mat<S>& m2)
|
|
{
|
|
|
|
assert(m1.ncols() == m2.ncols());
|
|
mat<T> r(m1.nrows()+m2.nrows(),m1.ncols());
|
|
for(unsigned i = 0; i < m1.nrows();i++)
|
|
{
|
|
for(unsigned j = 0; j < m1.ncols(); j++)
|
|
r(i,j) = m1(i,j);
|
|
}
|
|
|
|
for(unsigned i = 0; i < m2.nrows();i++)
|
|
{
|
|
for(unsigned j = 0; j < m1.ncols(); j++)
|
|
r(i+m1.nrows(),j) = m2(i,j);
|
|
}
|
|
|
|
return r;
|
|
}
|
|
|
|
|
|
|
|
|
|
//transpose of a matrix m
|
|
template <typename T>
|
|
const mat<T> transpose(const mat<T> &m)
|
|
{
|
|
mat<T> r = m;
|
|
r.transpose();
|
|
return r;
|
|
}
|
|
//transpose of a column vector
|
|
template <typename T>
|
|
const mat<T> transpose(const vec<T> &v)
|
|
{
|
|
mat<T> r(1, v.size());
|
|
for(unsigned i = 0; i < v.size(); i++)
|
|
r(0,i)= v(i);
|
|
|
|
return r;
|
|
}
|
|
|
|
///ceil all components of the matrix
|
|
template <typename T>
|
|
const mat<T> ceil(const mat<T> &m)
|
|
{
|
|
mat<T> r = m;
|
|
r.ceil();
|
|
return r;
|
|
}
|
|
|
|
///floor all components of the matrix
|
|
template <typename T>
|
|
const mat<T> floor(const mat<T> &m)
|
|
{
|
|
mat<T> r = m;
|
|
r.floor();
|
|
return r;
|
|
}
|
|
|
|
///round all components of the matrix
|
|
template <typename T>
|
|
const mat<T> round(const mat<T> &m)
|
|
{
|
|
mat<T> r = m;
|
|
r.round();
|
|
return r;
|
|
}
|
|
|
|
|
|
|
|
|
|
///return the product of a scalar s and a matrix m
|
|
template <typename T>
|
|
mat<T> operator * (const T& s, const mat<T>& m)
|
|
{
|
|
return m*(T)s;
|
|
}
|
|
|
|
|
|
|
|
///output of a matrix onto an ostream
|
|
template <typename T>
|
|
std::ostream& operator<<(std::ostream& out, const mat<T>& m)
|
|
{
|
|
|
|
for (unsigned i=0;i<m.nrows();++i)
|
|
{
|
|
for(unsigned j =0;j < m.ncols()-1;++j)
|
|
out << m(i,j)<<" ";
|
|
out << m(i,m.ncols()-1);
|
|
if(i != m.nrows()-1)
|
|
out <<"\n";
|
|
}
|
|
|
|
return out;
|
|
|
|
}
|
|
|
|
|
|
///input of a matrix onto an ostream
|
|
template <typename T>
|
|
std::istream& operator>>(std::istream& in, mat<T>& m)
|
|
{
|
|
assert(m.size() > 0);
|
|
for (unsigned i=0;i<m.nrows();++i)
|
|
for(unsigned j =0;j < m.ncols();++j)
|
|
in >> m(i,j);
|
|
|
|
|
|
return in;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//compute transpose(A)*A
|
|
template <typename T>
|
|
void AtA(const mat<T>& a, mat<T>& ata)
|
|
{
|
|
ata.resize(a.ncols(),a.ncols());
|
|
ata.zeros();
|
|
for(unsigned r = 0; r < a.nrows();r++)
|
|
{
|
|
for(unsigned i = 0; i < a.ncols();i++)
|
|
{
|
|
for(unsigned j = 0; j < a.ncols();j++)
|
|
{
|
|
ata(i,j)+=a(r,i)*a(r,j);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//compute A*transpose(A)
|
|
template <typename T>
|
|
void AAt(const mat<T>& a, mat<T>& aat)
|
|
{
|
|
aat.resize(a.nrows(),a.nrows());
|
|
aat.zeros();
|
|
for(unsigned c = 0; c < a.ncols();c++)
|
|
{
|
|
for(unsigned i = 0; i < a.nrows();i++)
|
|
{
|
|
for(unsigned j = 0; j < a.nrows();j++)
|
|
{
|
|
aat(i,j)+=a(i,c)*a(j,c);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
template <typename T>
|
|
mat<T> reshape(const vec<T>& v,unsigned r,unsigned c)
|
|
{
|
|
assert(v.size() == r*c);
|
|
return mat<T>(r,c,(const T*)v);
|
|
}
|
|
|
|
template <typename T>
|
|
mat<T> reshape(const mat<T>& m,unsigned r,unsigned c)
|
|
{
|
|
assert(m.size() == r*c);
|
|
return mat<T>(r,c,(const T*)m);
|
|
}
|
|
|
|
|
|
template <typename T>
|
|
void AtB(const mat<T>& a,const mat<T>& b, mat<T>& atb)
|
|
{
|
|
atb.resize(a.ncols(),b.ncols());
|
|
atb.zeros();
|
|
|
|
for(unsigned i = 0; i < a.ncols(); i++)
|
|
for(unsigned j = 0; j < b.ncols();j++)
|
|
for(unsigned k = 0; k < a.nrows(); k++)
|
|
atb(i,j) += a(k,i)*b(k,j);
|
|
|
|
}
|
|
|
|
///multiply A^T*x
|
|
/// A is a matrix and x is a vector
|
|
template <typename T>
|
|
void Atx(const mat<T>& a,const vec<T>& x, vec<T>& atx)
|
|
{
|
|
atx.resize(a.ncols());
|
|
atx.zeros();
|
|
|
|
for(unsigned i = 0; i < a.ncols(); i++)
|
|
for(unsigned j = 0; j < a.nrows(); j++)
|
|
atx(i) += a(j,i) * (T)(x(j));
|
|
|
|
|
|
}
|
|
|
|
|
|
///returns the outer product of vector v and w
|
|
template < typename T, typename S>
|
|
mat<T> dyad(const vec<T>& v, const vec<S>& w)
|
|
{
|
|
mat<T> m(v.size(),w.size());
|
|
|
|
for(unsigned i = 0; i < v.size();i++)
|
|
for(unsigned j = 0; j < w.size();j++)
|
|
m(i,j) =v(i)*(T)w(j);
|
|
|
|
return m;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|