diag_mat.h

#pragma once
 
#include "vec.h"
#include "mat.h"
#include "up_tri_mat.h"
 
namespace cgv{
    namespace math{
 
template <typename T>
struct diag_mat 
{
 
public:
    vec<T> _data;
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  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;
    
    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();}
 
    diag_iterator diag_begin(){return _data.begin();}
    diag_iterator diag_end(){return _data.end();}
    const_diag_iterator diag_begin() const{return _data.begin();}
    const_diag_iterator diag_end() const{return _data.end();}
    reverse_diag_iterator diag_rbegin(){return _data.rbegin();}
    reverse_diag_iterator diag_rend(){return _data.rend();}
    const_reverse_diag_iterator diag_rbegin() const{return _data.rbegin();}
    const_reverse_diag_iterator diag_rend() const {return _data.rend();}
 
    unsigned size() const 
    {
        return _data.size();
    }
 
    unsigned nrows() const
    {
        return size();
    }
 
    unsigned ncols() const
    {
        return size();
    }
 
    diag_mat()
    {
    }
 
    explicit diag_mat(unsigned n):_data(n)
    {
    }
 
    diag_mat(unsigned n, const T& c)
    {
        resize(n);
        fill(c);
    }
 
    //creates nxn diagonal matrix from array containing the diagonal elements
    diag_mat(unsigned n, const T* marray):_data(n,marray)
    {
    }
 
    diag_mat(const vec<T>& dv):_data(dv)
    {
    }
 
    diag_mat(const mat<T>& m)
    {
        int n = std::min(m.ncols(),m.nrows());
        resize(n);
        for(int i =0; i < n; i++)
            _data[i]=m(i,i);
    }
 
    diag_mat(const diag_mat<T>& m)
    {
        _data=m._data;
    }
 
    
 
    virtual ~diag_mat()
    {
    }
 
    T* data()
    {
        return _data.data();
    }
 
    const T* data() const
    {
        return _data.data();
    }
 
    diag_mat<T> sub_mat(unsigned top_left, unsigned size) const
    {
        diag_mat<T> D(size);
        for (unsigned int i=0; i<size; ++i)
            D(i) = (*this)(i+top_left);
        return D;
    }
    operator const mat<T>() const
    {
        mat<T> m;
        m.zeros(size(),size());
 
        for(unsigned i =0; i < size();i++)
                    m(i,i)=operator()(i);
        
        return m;
    }
    
 
    //resize the diagonal matrix to nxn
    void resize(unsigned n)
    {
        _data.resize(n);
    }
 
    void identity()
    {
        fill((T)1);
    }
 
    void fill(const T& c)
    {
        for(unsigned i=0;i < size();i++)
        {
            _data[i]=c;
        }
    }
    void zeros()
    {
        fill((T)0);
    }
 
    void exchange_diagonal_elements(unsigned i, unsigned j)
    {
        std::swap(_data[i],_data[j]);
    }
 
    const T& operator() (unsigned i) const 
    {
        return _data[i];
    }
 
    T& operator() (unsigned i) 
    {
        return _data[i];
    }
 
    const T& operator[] (unsigned i) const 
    {
        return _data[i];
    }
 
    T& operator[] (unsigned i) 
    {
        return _data[i];
    }
 
 
    bool is_square()
    {
        return true;
    }
 
    //transpose (does nothing)
    void transpose(){}
 
 
    diag_mat<T>& operator = (const diag_mat<T>& m) 
    {
        _data  =m._data;
        return *this;
    }
 
    template <typename S> 
    diag_mat<T>& operator = (const diag_mat<S>& m) 
    {  
        resize(m.size());
        for(unsigned i = 0; i < size(); i++)
                _data[i]=(T)m(i); 
        return *this;
    }
 
    template <typename S> 
    diag_mat<T>& operator = (const vec<S>& v) 
    {  
        _data=v; 
        return *this;
    }
 
    diag_mat<T>& operator  = (const T& s) 
    { 
        fill (s);
        return *this; 
    }
    T frobenius_norm() const
    {
        T n=0;
        for(int i =0; i < size();i++)
            n+=_data[i]*_data[i];
    
        return (T)sqrt((double)n);
    }
 
 
    
    
    void identity(unsigned dim)
    {
        resize(dim);
        for(unsigned i = 0; i < size();++i)
            _data[i]=1;
    }
 
    void zero()
    {
        fill(0);
    }
 
    diag_mat<T>& operator+=(const diag_mat<T>& d) 
    {
        assert(d.nrows() == nrows());
        _data += d._data;
        return *this;
    }
 
    diag_mat<T>& operator-=(const diag_mat<T>& d)
    {
        assert(d.nrows() == nrows());
        _data -= d._data;
        return *this;
    }
 
    diag_mat<T>& operator*=(const T& s)
    {
        _data *=s;
        return *this;
    }
 
    diag_mat<T> operator*(const T& s) const
    {
        diag_mat<T> r=*this;
        r*=s;
        return r;
    }
 
    diag_mat<T> operator+(const diag_mat<T>& d) const
    {
        diag_mat<T> r=*this;
        r += d;
        return r;
    }
 
    diag_mat<T> operator-(const diag_mat<T>& d) const
    {
        diag_mat<T>r = *this;
        r-= d;
        return r;
    }
 
    
 
    vec<T> operator*(const vec<T>& v) const
    {
        assert(v.dim() == ncols());
        vec<T> r(size());
        for(unsigned i = 0; i < size();i++)
            r(i) = _data[i]*v(i);
        return r;
    }
 
    const up_tri_mat<T> operator*( const up_tri_mat<T>& m) 
    {       
        assert(m.nrows() == size());
 
        up_tri_mat<T> r(size());
 
        for(unsigned i = 0; i < size(); i++)
            for(unsigned j = 0; j < m.ncols();j++)
                r(i,j) = operator()(i)*m(i,j);
 
        return r;
    }
 
    operator mat<T>()
    {
        mat<T> m(size(),size(),(T)0);
        for(unsigned i = 0; i < size();i++)
        {
            m(i,i)=_data[i];
        }
        return m;
    }
 
};
 
//matrix multiplication  of matrix m by a diagonal matrix s
template <typename T, typename S>
const mat<T> operator*(const mat<T>& m,const diag_mat<S>& s) 
{       
    assert(m.ncols() == s.size());
    mat<T> r(m.nrows(),s.size());
    for(unsigned i = 0; i < m.nrows(); i++)
        for(unsigned j = 0; j < s.size();j++)
            r(i,j) = s(j)*m(i,j);
 
    return r;
}
 
template <typename T>
const diag_mat<T> operator*(const T& s, const diag_mat<T>& m)
{
    return m*s;
}
 
template <typename T, typename S>
const diag_mat<T> operator*(const diag_mat<S>& s, const diag_mat<T>& m) 
{       
    assert(m.size() == s.size());
 
    diag_mat<T> r(s.size());
 
    for(unsigned i = 0; i < s.size(); i++)
            r(i) = s(i)*m(i);
 
    return r;
}
template<typename T>
mat<T> operator*(const perm_mat& p, const diag_mat<T>& m)
{
    mat<T> r=m;
    return p*r;
}
 
template<typename T>
mat<T> operator*(const diag_mat<T>& m,const perm_mat& p)
{
    mat<T> r=m;
    return r*p;
}
 
template <typename T>
const mat<T> operator*(const diag_mat<T>& s, const mat<T>& m) 
{       
    assert(m.nrows() == s.size());
 
    mat<T> r(s.size(),m.ncols());
 
    for(unsigned i = 0; i < s.size(); i++)
        for(unsigned j = 0; j < m.ncols();j++)
            r(i,j) = s(i)*m(i,j);
 
    return r;
}
 
template <typename T>
const up_tri_mat<T> operator*(const up_tri_mat<T>& m,const diag_mat<T>& s) 
{       
    assert(m.ncols() == s.size());
    up_tri_mat<T> r(s.size());
    for(unsigned i = 0; i < m.nrows(); i++)
        for(unsigned j = i; j < s.size();j++)
            r(i,j) = s(j)*m(i,j);
 
    return r;
}
 
 
 
 
 
template <typename T>
std::ostream& operator<<(std::ostream& out, const diag_mat<T>& m)
{
    
    for (int i=0;i<(int)m.size();++i)
        out << m(i)<<" ";
 
    return out;
}
 
template <typename T>
std::istream& operator>>(std::istream& in,  diag_mat<T>& m)
{
    assert(m.size() > 0);
    for (int i=0;i<(int)m.size();++i)
        in >> m(i);
 
    return in;
}
 
 
}
}