|| ||           || Doxygen's  

---

HxMatrix.h

/*
 *  Copyright (c) 1998, University of Amsterdam, The Netherlands.
 *  All rights reserved.
 *
 *
 *  Author(s):
 *  Dennis Koelma (koelma@wins.uva.nl)
 *  Edo Poll (poll@wins.uva.nl)
 *  Jan-Mark Geusebroek (poll@wins.uva.nl)
 *  Jan van Gemert (jvgemert@science.uva.nl)
 */


// Template code in the header file.
// because this yields the most generic patterns, and avoids dead code.

#ifndef HxTMatrix_h
#define HxTMatrix_h

#include "HxIo.h"
#include "HxStd.h"

#include "HxVec3Double.h"

//declare the HxVector class.
class L_HXBASIS HxVector;


template<class C> class L_HXBASIS HxTMatrix {
public:

    HxTMatrix<C>() ;

    HxTMatrix<C>(int nrow, int ncol);

    HxTMatrix<C>(int nrow, int ncol, C a);

    HxTMatrix<C>(int nrow, int ncol, C* data);

    HxTMatrix<C>(const HxString fileName, bool bin=true);

    HxTMatrix<C>(const HxTMatrix &m) ;

    HxTMatrix<C>(const HxVector& v);

//  template<class C, class U>
//  friend HxTMatrix<C> conv(const HxTMatrix<U> &m) ;
// if I unComment this, I get an internal compilor error.. what to do ?


    // Destructor
    ~HxTMatrix<C>() ;

    static HxTMatrix<C>     translate2d(C x, C y);

    static HxTMatrix<C>     scale2d(double sx, double sy);

    static HxTMatrix<C>     rotate2d(double alpha);

    static HxTMatrix<C>     rotate2dDeg(double alpha);

    static HxTMatrix<C>     reflect2d(int doX, int doY);

    static HxTMatrix<C>     shear2d(double sx, double sy);

    static HxTMatrix<C>     translate3d(double x, double y, double z);

    static HxTMatrix<C>     scale3d(double sx, double sy, double sz);

    static HxTMatrix<C>     rotateX3d(double alpha);

    static HxTMatrix<C>     rotateX3dDeg(double alpha);

    static HxTMatrix<C>     rotateY3d(double alpha);

    static HxTMatrix<C>     rotateY3dDeg(double alpha);

    static HxTMatrix<C>     rotateZ3d(double alpha);

    static HxTMatrix<C>     rotateZ3dDeg(double alpha);

    static HxTMatrix<C>     reflect3d(int doX, int doY, int doZ);

    static HxTMatrix<C>     projection(double f);

    static HxTMatrix<C>     camera(double f);

    static HxTMatrix<C>     lift2dTo3dXY();


    int             nRow()  const;

    int             nCol()  const;

    int             nElem() const;

    int             valid() const;

    HxVector        getRow(long r) const;

    HxVector        getCol(long c) const;




    HxTMatrix<C>        setRow(long r, HxVector v, bool makeCopy=false) ;

    HxTMatrix<C>        setCol(long c, HxVector v, bool makeCopy=false) ;

    HxTMatrix<C>&       operator=(C a);

    HxTMatrix<C>&       operator=(const HxTMatrix<C>& m);

    C*                  operator[](int i) const;


    HxTMatrix<C>        operator-() const;

    HxTMatrix<C>        operator*(double          val) const ;

    HxTMatrix<C>        operator*(const HxTMatrix<C>& a) const ;

    friend HxVector     operator*(const HxVector& , const HxTMatrix<C>& );

    friend HxVector     operator*(const HxTMatrix<C>& , const HxVector& );

    HxTMatrix<C>        operator/(double val) const ;

    friend HxTMatrix<C> operator/(double a, const HxTMatrix<C>& b);

    HxTMatrix<C>        operator+(const HxTMatrix<C>& a) const ;

    HxTMatrix<C>        operator+(double          val) const ;

    HxTMatrix<C>        operator-(const HxTMatrix<C>& a) const ;

    HxTMatrix<C>        operator-(double          val) const ;

    int                 operator==(const HxTMatrix<C>& a) const ;

    int                 operator!=(const HxTMatrix<C>& a) const ;

    HxVec3Double        operator*(const HxVec3Double& b) const;



    HxTMatrix<C>        delRow(long r) const;

    HxTMatrix<C>        delCol(long c) const;

    void                write2disk(const HxString fileName, bool bin=true) const ;


    HxTMatrix<C>        i() const;

    HxTMatrix<C>        t() const;

    HxTMatrix<C>        covariance(HxVector &mean=HxVector()) const ;

    HxTMatrix<C>        svd(HxVector& W, HxTMatrix<C>& V) const;

    HxTMatrix<C>        klm(int newDim) const;

    void                kmeans(HxTMatrix<C> &c, HxVector &imap, int numloop, bool cIsInitialized) const ;


    HxTMatrix<C>        add(const HxTMatrix<C>& b) const;


    HxTMatrix<C>        add(const double val) const;


    HxTMatrix<C>        sub(const HxTMatrix<C>& b) const;

    HxTMatrix<C>        sub(const double val) const;

    HxTMatrix<C>        mul(const HxTMatrix<C>& b) const;

    HxTMatrix<C>        mul(const HxVector& v) const;

    HxTMatrix<C>        mul(const double val) const;

    HxTMatrix<C>        div(const double val) const;

    HxTMatrix<C>        sin() const;

    HxTMatrix<C>        cos() const;

    HxTMatrix<C>        tan() const;

    HxTMatrix<C>        sinh() const;

    HxTMatrix<C>        cosh() const;

    HxTMatrix<C>        tanh() const;

    HxTMatrix<C>        exp() const;

    HxTMatrix<C>        log() const;

    HxTMatrix<C>        sqrt() const;

    HxTMatrix<C>        abs() const;

    HxTMatrix<C>        sgn() const;

    HxTMatrix<C>        map(C (*f)(C)) const;

    HxTMatrix<C>        app(const HxTMatrix<C>& b) const;


    STD_OSTREAM&    put(STD_OSTREAM& os) const;


private:

    friend class HxVector ;
    int     _nr;    // number of rows
    int     _nc;    // number of columns
    C*      _data;  // data pointer

};


// declare HxMatrix
L_HXBASIS typedef HxTMatrix<double>  HxMatrix ;


//#include <cmath>
#include "HxEnvironment.h"

template<class C> 
static void svdcmp(C* a, HxVector &w, C* v, int n, int m);

template<class C> 
static int ludcmp(C* a, int n, short *indx, double *d);

template<class C> 
static void lubksb(C* a, int n, short *indx, double *b);

#ifndef HxMatrix_EPS_DEF 
#define HxMatrix_EPS_DEF
const double HxMatrix_EPS = 1e-12;
#endif

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#ifndef HxMatrix_Error 
#define HxMatrix_Error
static void
error(HxString msg)
{
    HxEnvironment::instance()->errorStream() << "error: " << msg << STD_ENDL;
    HxEnvironment::instance()->flush();
}
#endif


//---------------------------------------------------
//      Construction:
//---------------------------------------------------

template<class C> inline
HxTMatrix<C>::HxTMatrix<C>()
{
    _nr = 0;
    _nc = 0;
    _data = 0;
}



template<class C> inline
HxTMatrix<C>::HxTMatrix<C>(int nRow, int nCol)
{
    _nr = nRow;
    _nc = nCol;
    _data = new C[_nr * _nc];
}



template<class C> inline
HxTMatrix<C>::HxTMatrix<C>(int nRow, int nCol, C a)
{
    _nr = nRow;
    _nc = nCol;
    _data = new C[_nr * _nc];

    C* t = _data;
    int i = nElem();
    while (--i >= 0)
        *t++ = a;

}


template<class C> inline
HxTMatrix<C>::HxTMatrix<C>(int nRow, int nCol, C* data)
{
    _nr = nRow;
    _nc = nCol;
    _data = data;
}

template<class C> inline
HxTMatrix<C>::HxTMatrix<C>(HxString fileName, bool bin)
{

    
    FILE *fp;

    if(bin) {   // read binary file

        fp = fopen(fileName.c_str(), "rb") ;

        // read the header of the matrix
        fread(&_nr, sizeof(int), 1, fp) ;
        fread(&_nc, sizeof(int), 1, fp) ;

        _data = new C[_nr * _nc];

        // read data
        fread(_data,sizeof(C),_nr*_nc,fp) ;

    } 
    else {  // read text file

        fp = fopen(fileName.c_str(), "r") ;

        //read the header of the matrix
        fscanf(fp, "%d %d", &_nr, &_nc);
    //  printf("nr=%d, nc=%d\n",_nr,_nc) ;

        _data = new C[_nr * _nc];

        double d ;
        for(int i=0; i<_nr;i++) {
            for(int j=0; j<_nc;j++) {
                fscanf(fp, "%lf ", &d) ;
                (*this)[i][j] = (C) d ;
            }
        }

    }
    fclose(fp) ;

}



template<class C> inline
HxTMatrix<C>::HxTMatrix<C>(const HxTMatrix& m)
{
    _nr = m.nRow();
    _nc = m.nCol();
    _data = new C[_nr * _nc];

    C* t = m._data;
    C* u = _data;
    int i = m.nElem();
    while (--i >= 0)
        *u++ = *t++;
}

/*
template<class C> inline
HxTMatrix<C>::HxTMatrix<C>(const HxMatrix& m)
{

    _nr = m.nRow();
    _nc = m.nCol();
    _data = new C[_nr * _nc];

    for(int r=0; r<m.nRow(); r++)
        for(int c=0; c<m.nCol(); c++)
            (*this)[r][c] = m[r][c] ;
}
*/


template<class C, class U> inline
HxTMatrix<C> conv(const HxTMatrix<U> &m)
{
    HxTMatrix<C> tmp(m.nRow(), m.nCol()) ;

    for(int r=0; r<m.nRow(); r++)
        for(int c=0; c<m.nCol(); c++)
            tmp[r][c] = m[r][c] ;
/*
    C* t = m._data;
    C* u = tmp._data;
    int i = m.nElem();
    while (--i >= 0)
        *u++ = *t++;
*/
    return tmp;

}

/*
template<class C, class U>
HxTMatrix<C>::HxTMatrix(const HxTMatrix<U>& m )
{
    HxTMatrix<C> tmp(m.nRow(), m.nCol(), (C) 0) ;

    for(int r=0; r<m.nRow(); r++)
        for(int c=0; c<m.nCol(); c++)
            tmp[r][c] = m[r][c] ;
    return tmp;

    return conv<C>(m) ;
}
*/

template<class C> inline
HxTMatrix<C>::~HxTMatrix<C>()
{
    delete[] _data ;
}

template<class C> STD_OSTREAM&
HxTMatrix<C>::put(STD_OSTREAM& os) const
{
    C* t = _data;
    int i = nRow();
    while (--i >= 0) {
        int j = nCol();
        os << "| ";
        while (--j >= 0)
            os << (double) *t++ << " ";
        os << "|" << STD_ENDL;
    }
    return os;
}


//---------------------------------------------------
//      Inquiry:
//---------------------------------------------------

#include "HxVector.h"

template<class C> inline int
HxTMatrix<C>::nRow() const
{
    return _nr;
}

template<class C> inline int
HxTMatrix<C>::nCol() const
{
    return _nc;
}

template<class C> inline int
HxTMatrix<C>::nElem() const
{
    return _nr * _nc;
}

template<class C> inline int
HxTMatrix<C>::valid() const
{
    return ((_nc != 0) && (_nr != 0));
}

template<class C> inline HxVector
HxTMatrix<C>::getRow(long r) const
{

    if(r>= _nr || r<0){
        error("getRow: rownr exceeded the number of rows in HxTMatrix<C>.");
        return HxVector(0);
    }

    HxVector tmp(_nc);

    for(int i = 0; i<_nc; i++)
        tmp[i] = (*this)[r][i] ;

    return tmp ;
}

template<class C> inline HxVector
HxTMatrix<C>::getCol(long c) const
{

    if(c>= _nc || c<0){
        error("getCol: Colnr exceeded number of columns in HxTMatrix<C>.");
        return HxVector(0);
    }

    HxVector tmp(_nr);
    for(int i = 0; i<_nr; i++)
    {
        tmp[i] = (*this)[i][c] ;
    }
    return tmp;

}


template<class C> inline HxTMatrix<C> 
HxTMatrix<C>::setRow(long r, HxVector v, bool makeCopy) 
{

    if(r>= _nr || r<0){
        error("setRow: rownr exceeded the number of rows in HxTMatrix<C>.");
        return HxTMatrix<C>(0,0) ;
    }
    if(_nc != v.nElem()){
        error("setRow: v.nElem not equal to matrix dimension");
        return HxTMatrix<C>(0,0) ;
    }

    HxTMatrix<C> tmp(0,0);

    if(makeCopy) {

        tmp = HxTMatrix<C>(_nr,_nc);

        for(int row = 0; row<_nr; row++)
            for(int col = 0; col<_nc; col++)
                if(row!=r) tmp[row][col] = (*this)[row][col] ;
                else tmp[row][col] = v[col] ;
    } 
    else { // only change the original, faster

        for(int col = 0; col<_nc; col++)
            (*this)[r][col] = v[col] ;
    }


    return tmp;

}

template<class C> inline HxTMatrix<C> 
HxTMatrix<C>::setCol(long c, HxVector v, bool makeCopy) 
{

    if(c>= _nc || c<0){
        error("setCol: Colnr exceeded number of columns in HxTMatrix<C>.");
        return HxTMatrix<C>(0,0) ;
    }
    if(_nr != v.nElem()){
        error("setCol: v.nElem not equal to matrix dimension");
        return HxTMatrix<C>(0,0) ;
    }

    HxTMatrix<C> tmp(0,0);

    if(makeCopy) {

        tmp = HxTMatrix<C>(_nr,_nc);

        for(int row = 0; row<_nr; row++)
            for(int col = 0; col<_nc; col++)
                if(col!=c) tmp[row][col] = (*this)[row][col] ;
                else tmp[row][col] = v[row] ;
    }
    else { // only change the original, faster

        for(int row = 0; row<_nr; row++)
            (*this)[row][c] = v[row] ;
    }

    return tmp;

}
template<class C> inline HxTMatrix<C> 
HxTMatrix<C>::delRow(long r) const
{

    if(r>= _nr || r <0){
        error("delRow: rownr exceeded the number of rows in HxTMatrix<C>.");
        return HxTMatrix<C>(0,0) ;
    }

    HxTMatrix<C> tmp(_nr-1,_nc);

    for(int row = 0; row<_nr; row++)
        for(int col = 0; col<_nc; col++)
            if(row < r) tmp[row][col] = (*this)[row][col] ;
            else if(row > r) tmp[row-1][col] = (*this)[row][col] ;

    return tmp ;

}


template<class C> inline HxTMatrix<C> 
HxTMatrix<C>::delCol(long c) const
{

    if(c>= _nc || c<0){
        error("delCol: colnr exceeded the number of cols in HxTMatrix<C>.");
        return HxTMatrix<C>(0,0) ;
    }

    HxTMatrix<C> tmp(_nr,_nc-1);

    for(int row = 0; row<_nr; row++)
        for(int col = 0; col<_nc; col++)
            if(col < c) tmp[row][col] = (*this)[row][col] ;
            else if(col > c) tmp[row][col-1] = (*this)[row][col] ;

    return tmp ;

}


//---------------------------------------------------
//      Operators:
//---------------------------------------------------

template<class C> inline HxTMatrix<C>&
HxTMatrix<C>::operator=(C a)
{
    int i = nElem();
    C *t = _data;
    while (--i >= 0)
        *t++ = a;
    return *this;
}



template<class C> inline HxTMatrix<C>&
HxTMatrix<C>::operator=(const HxTMatrix<C>& m)
{
    if (this != &m) {
        delete [] _data;
        _nr = m.nRow();
        _nc = m.nCol();
        _data = new C [_nr * _nc];
        C *t = _data;
        C *u = m._data;
        int i = m.nElem();
        while (--i >= 0)
            *t++ = *u++;
    }
    return *this;
}


template<class C> inline C*
HxTMatrix<C>::operator[](int i) const
{
    return &_data[i*_nc];
}


template<class C> inline HxTMatrix<C>
HxTMatrix<C>::operator-() const
{
    HxTMatrix<C> m(*this);
    C* t = m._data;
    C* u = _data;
    int i = nElem();
    while (--i >= 0)
        *t++ = -(*u++);
    return m;
}

template<class C> inline HxTMatrix<C>
HxTMatrix<C>::operator*(double val) const
{
    HxTMatrix<C> m(*this);
    C* t = m._data;
    int i = nElem();
    while (--i >= 0)
        *t++ = *t * val ;
    return m;
}

template<class C> inline HxTMatrix<C>
operator*(double a, const HxTMatrix<C> &b)
{
    return b * a ;
}


template<class C> inline HxTMatrix<C>
HxTMatrix<C>::operator/(double val) const
{
    HxTMatrix<C> m(*this);
    C* t = m._data;
    int i = nElem();
    while (--i >= 0)
        *t++ = *t / val ;
    return m;}

template<class C> inline HxTMatrix<C>
operator/(double a, const HxTMatrix<C> &b)
{
    HxTMatrix<C> m(b);
    C* t = m._data;
    C* u = b._data;
    int i = b.nElem();
    while (--i >= 0)
        *t++ = a / *u++;
    return m;
}


template<class C> inline HxTMatrix<C>
HxTMatrix<C>::operator+(double val) const
{

    HxTMatrix<C> m(*this);
    C* t = m._data;
    int i = nElem();
    while (--i >= 0)
        *t++ = val + *t;
    return m;
}


template<class C> inline HxTMatrix<C>
operator+(double a, const HxTMatrix<C> &b)
{
    return b + a ;
}


template<class C> inline HxTMatrix<C>
HxTMatrix<C>::operator-(double val) const
{

    HxTMatrix<C> m(*this);
    C* t = m._data;
    int i = nElem();
    while (--i >= 0)
        *t++ = *t - val ;
    return m;
}

template<class C> inline HxTMatrix<C>
operator-(double val, const HxTMatrix<C> &b)
{
    return -b + val;
}


template<class C> inline int
HxTMatrix<C>::operator!=(const HxTMatrix<C> &a) const
{
    return !(a == *this);
}


template<class C> inline STD_OSTREAM&
operator<<(STD_OSTREAM& os, const HxTMatrix<C> v)
{
    return v.put(os);
}

//---------------------------------------------------
//      Operations:
//---------------------------------------------------

template<class C> inline HxTMatrix<C>
HxTMatrix<C>::t() const
{
    HxTMatrix<C> m(nCol(), nRow());
    int i, j;
    for (i=0 ; i<nRow() ; i++)
        for (j=0 ; j<nCol() ; j++)
            m[j][i] = (*this)[i][j];
    return m;
}


template<class C> inline HxTMatrix<C>
HxTMatrix<C>::map(C (*f)(C)) const
{
    HxTMatrix<C> m(*this);
    C* t = m._data;
    C* u = _data;
    int i = nElem();
    while (--i >= 0)
            *t++ = (C) f(*u++);
    return m;
}


//L_HXBASIS HxMatrix     operator*(const HxMatrix& a, const HxMatrix& b);
//template<class C> L_HXBASIS HxVector
//operator*(const HxVector& a, const HxTMatrix<C>& b);
//template<class C>
//L_HXBASIS HxVector     operator*(const HxTMatrix<C> &b, const HxVector& a);
//L_HXBASIS HxMatrix     operator+(const HxMatrix& a, const HxMatrix& b);
//L_HXBASIS HxMatrix     operator-(const HxMatrix& a, const HxMatrix& b);
//L_HXBASIS int          operator==(const HxMatrix& a, const HxMatrix& b);
//L_HXBASIS HxVec3Double operator*(const HxVec3Double& a, const HxMatrix& b);
//L_HXBASIS HxVec3Double operator*(const HxMatrix& a, const HxVec3Double& b);




//---------------------------------------------------
//      Construction:
//---------------------------------------------------

template<class C> 
HxTMatrix<C>::HxTMatrix<C>(const HxVector& v)
{
    _nc = v.nElem();
    _nr = 1;
    _data = new C[_nr * _nc];

    //int t = _nc;
    C* u = _data;
    int i = 0;
    while (i < _nc)
        *u++ = v[i++];
}

//---------------------------------------------------
//      HxTMatrix generation:
//---------------------------------------------------

template<class C> HxTMatrix<C> 
HxTMatrix<C>::translate2d(C x, C y)
{
    HxTMatrix<C> m(3,3);
    m[0][0] = 1; m[0][1] = 0; m[0][2] = x;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = y;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 1;
    /* prefix:
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;
    m[2][0] = x; m[2][1] = y; m[2][2] = 1;
    */
    return m;
}



template<class C> HxTMatrix<C>
HxTMatrix<C>::scale2d(double sx, double sy)
{
    HxTMatrix<C> m(3,3);
    m[0][0] = sx; m[0][1] =  0; m[0][2] = 0;
    m[1][0] =  0; m[1][1] = sy; m[1][2] = 0;
    m[2][0] =  0; m[2][1] =  0; m[2][2] = 1;
    return m;
}


template<class C> HxTMatrix<C> 
HxTMatrix<C>::rotate2d(double alpha) // alpha in rad
{
    HxTMatrix<C> m(3,3);
    m[0][0] = ::cos(alpha); m[0][1] = -::sin(alpha); m[0][2] = 0;
    m[1][0] = ::sin(alpha); m[1][1] = ::cos(alpha);  m[1][2] = 0;
    m[2][0] = 0;            m[2][1] = 0;             m[2][2] = 1;
    /* prefix:
    m[0][0] =  ::cos(alpha); m[0][1] = ::sin(alpha); m[0][2] = 0;
    m[1][0] = -::sin(alpha); m[1][1] = ::cos(alpha); m[1][2] = 0;
    m[2][0] = 0;             m[2][1] = 0;            m[2][2] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::rotate2dDeg(double alpha) // alpha in deg
{
    return rotate2d(M_PI*alpha/180.0);
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::reflect2d(int doX, int doY)
{
    double rx = (doX) ? -1 : 1;
    double ry = (doY) ? -1 : 1;
    HxTMatrix<C> m(3,3);
    m[0][0] = rx; m[0][1] =  0; m[0][2] = 0;
    m[1][0] =  0; m[1][1] = ry; m[1][2] = 0;
    m[2][0] =  0; m[2][1] =  0; m[2][2] = 1;
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::shear2d(double sx, double sy)
{
    HxTMatrix<C> m(3,3);
    m[0][0] =  1; m[0][1] = sx; m[0][2] = 0;
    m[1][0] = sy; m[1][1] =  1; m[1][2] = 0;
    m[2][0] =  0; m[2][1] =  0; m[2][2] = 1;
    /* prefix:
    m[0][0] =  1; m[0][1] = sy; m[0][2] = 0;
    m[1][0] = sx; m[1][1] =  1; m[1][2] = 0;
    m[2][0] =  0; m[2][1] =  0; m[2][2] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::translate3d(double x, double y, double z)
{
    HxTMatrix<C> m(4,4);
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = x;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = y;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = z;
    m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
    /* prefix:
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
    m[3][0] = x; m[3][1] = y; m[3][2] = z; m[3][3] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::scale3d(double sx, double sy, double sz)
{
    HxTMatrix<C> m(4,4);
    m[0][0] = sx; m[0][1] =  0; m[0][2] =  0; m[0][3] = 0;
    m[1][0] =  0; m[1][1] = sy; m[1][2] =  0; m[1][3] = 0;
    m[2][0] =  0; m[2][1] =  0; m[2][2] = sz; m[2][3] = 0;
    m[3][0] =  0; m[3][1] =  0; m[3][2] =  0; m[3][3] = 1;
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::rotateX3d(double alpha) // alpha in rad
{
    HxTMatrix<C> m(4,4);
    m[0][0] = 1; m[0][1] = 0;            m[0][2] = 0;             m[0][3] = 0;
    m[1][0] = 0; m[1][1] = ::cos(alpha); m[1][2] = -::sin(alpha); m[1][3] = 0;
    m[2][0] = 0; m[2][1] = ::sin(alpha); m[2][2] = ::cos(alpha);  m[2][3] = 0;
    m[3][0] = 0; m[3][1] = 0;            m[3][2] = 0;             m[3][3] = 1;
    /* prefix
    m[0][0] = 1; m[0][1] = 0;             m[0][2] = 0;            m[0][3] = 0;
    m[1][0] = 0; m[1][1] = ::cos(alpha);  m[1][2] = ::sin(alpha); m[1][3] = 0;
    m[2][0] = 0; m[2][1] = -::sin(alpha); m[2][2] = ::cos(alpha); m[2][3] = 0;
    m[3][0] = 0; m[3][1] = 0;             m[3][2] = 0;            m[3][3] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::rotateX3dDeg(double alpha) // alpha in deg
{
    return rotateX3d(M_PI*alpha/180.0);
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::rotateY3d(double alpha) // alpha in rad
{
    HxTMatrix<C> m(4,4);
    alpha = M_PI*alpha/180.0;
    m[0][0] = ::cos(alpha);  m[0][1] = 0; m[0][2] = ::sin(alpha); m[0][3] = 0;
    m[1][0] = 0;             m[1][1] = 1; m[1][2] = 0;            m[1][3] = 0;
    m[2][0] = -::sin(alpha); m[2][1] = 0; m[2][2] = ::cos(alpha); m[2][3] = 0;
    m[3][0] = 0;             m[3][1] = 0; m[3][2] = 0;            m[3][3] = 1;
    /* prefix:
    m[0][0] = ::cos(alpha); m[0][1] = 0; m[0][2] = -::sin(alpha); m[0][3] = 0;
    m[1][0] = 0;            m[1][1] = 1; m[1][2] = 0;             m[1][3] = 0;
    m[2][0] = ::sin(alpha); m[2][1] = 0; m[2][2] = ::cos(alpha);  m[2][3] = 0;
    m[3][0] = 0;            m[3][1] = 0; m[3][2] = 0;             m[3][3] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::rotateY3dDeg(double alpha) // alpha in deg
{
    return rotateY3d(M_PI*alpha/180.0);
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::rotateZ3d(double alpha) // alpha in rad
{
    HxTMatrix<C> m(4,4);
    m[0][0] = ::cos(alpha); m[0][1] = -::sin(alpha); m[0][2] = 0; m[0][3] = 0;
    m[1][0] = ::sin(alpha); m[1][1] = ::cos(alpha);  m[1][2] = 0; m[1][3] = 0;
    m[2][0] = 0;            m[2][1] = 0;             m[2][2] = 1; m[2][3] = 0;
    m[3][0] = 0;            m[3][1] = 0;             m[3][2] = 0; m[3][3] = 1;
    /* prefix:
    m[0][0] = ::cos(alpha);  m[0][1] = ::sin(alpha); m[0][2] = 0; m[0][3] = 0;
    m[1][0] = -::sin(alpha); m[1][1] = ::cos(alpha); m[1][2] = 0; m[1][3] = 0;
    m[2][0] = 0;             m[2][1] = 0;            m[2][2] = 1; m[2][3] = 0;
    m[3][0] = 0;             m[3][1] = 0;            m[3][2] = 0; m[3][3] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::rotateZ3dDeg(double alpha) // alpha in deg
{
    return rotateZ3d(M_PI*alpha/180.0);
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::reflect3d(int doX, int doY, int doZ)
{
    double rx = (doX) ? -1 : 1;
    double ry = (doY) ? -1 : 1;
    double rz = (doZ) ? -1 : 1;
    HxTMatrix<C> m(4,4);
    m[0][0] = rx; m[0][1] =  0; m[0][2] =  0; m[0][3] = 0;
    m[1][0] =  0; m[1][1] = ry; m[1][2] =  0; m[1][3] = 0;
    m[2][0] =  0; m[2][1] =  0; m[2][2] = rz; m[2][3] = 0;
    m[3][0] =  0; m[3][1] =  0; m[3][2] =  0; m[3][3] = 1;
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::projection(double f)
{
    HxTMatrix<C> m(4,4);
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 1/f;
    m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
    /* prefix:
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;   m[0][3] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;   m[1][3] = 0;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 1;   m[2][3] = 0;
    m[3][0] = 0; m[3][1] = 0; m[3][2] = 1/f; m[3][3] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::camera(double f)
{
    HxTMatrix<C> m(3,4);
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;   m[0][3] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;   m[1][3] = 0;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 1/f; m[2][3] = 1;
    /* prefix:
    HxTMatrix<C> m(4,3);
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 1/f;
    m[3][0] = 0; m[3][1] = 0; m[3][2] = 1;
    */
    return m;
}

template<class C> HxTMatrix<C>
HxTMatrix<C>::lift2dTo3dXY()
{
    HxTMatrix<C> m(4,3);
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 0;
    m[3][0] = 0; m[3][1] = 0; m[3][2] = 1;
    /* prefix:
    HxTMatrix<C> m(3,4);
    m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
    m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
    m[2][0] = 0; m[2][1] = 0; m[2][2] = 0; m[2][3] = 1;
    */
    return m;
}

//---------------------------------------------------
//      Operators:
//---------------------------------------------------

template<class C> inline int 
HxTMatrix<C>::operator==(const HxTMatrix<C>& a) const 
{
    if ((a.nCol() != nCol()) || (a.nRow() != nRow()))
        return 0;

    C* t = _data;
    C* u = a._data;
    int i = a.nElem();
    while (--i >= 0) {
        if (fabs(*t++ - *u++) > HxMatrix_EPS)
            return 0;
    }
    return 1;

}


template<class C> HxTMatrix<C> 
HxTMatrix<C>::operator+(const HxTMatrix<C> &a) const 
{
    if ((a.nCol() != nCol()) || (a.nRow() != nRow()) ) {
        error("nonconformant HxTMatrix<C> + HxTMatrix<C> operands.");
        return HxTMatrix<C>(0,0);
    }
    HxTMatrix<C> m(nRow(), nCol());
    int i, j;
    for (i=0 ; i<nRow() ; i++) {
        for (j=0 ; j<nCol() ; j++)
            m[i][j] = a[i][j] + (*this)[i][j];
    }
    return m;
}


template<class C> HxTMatrix<C> 
HxTMatrix<C>::operator-(const HxTMatrix<C> &a) const 
{
    if ((a.nCol() != nCol()) || (a.nRow() != nRow()) ) {
        error("nonconformant HxTMatrix<C> + HxTMatrix<C> operands.");
        return HxTMatrix<C>(0,0);
    }
    HxTMatrix<C> m(nRow(), nCol());
    int i, j;
    for (i=0 ; i<nRow() ; i++) {
        for (j=0 ; j<nCol() ; j++)
            m[i][j] = (*this)[i][j] - a[i][j] ;
    }
    return m;
}


template<class C> HxTMatrix<C>
HxTMatrix<C>::operator*(const HxTMatrix<C> &a) const 
{
    if (nCol() != a.nRow()) {
        error("nonconformant HxTMatrix<C> * HxTMatrix<C> operands.");
        return HxTMatrix<C>(0,0);
    }
    HxTMatrix<C> m((*this).nRow(), a.nCol(), 0.0);
    double sum;
    int i, j, k;
    for (i=0 ; i<nRow() ; i++) {
        for (j=0 ; j<a.nCol() ; j++) {
            sum = 0;
            for (k=0 ; k<nCol() ; k++)
                sum += (*this)[i][k] * a[k][j];
            m[i][j] = sum;
        }
    }
    return m;
}
/*
template<class C> HxVector 
operator*(const HxVector &a, const HxTMatrix<C> &b)
{
    if (a.nElem() != b.nRow()) {
        error("nonconformant HxVector * HxTMatrix<C> operands.");
        return HxVector(0);
    }
    HxVector v(b.nCol());
    double sum;
    int i, j;
    for (i=0 ; i<b.nCol() ; i++) {
        sum = 0;
        for (j=0 ; j<b.nRow() ; j++)
            sum += a[j] * b[j][i];
        v[i] = sum;
    }
    return v;
}


*/
template<class C> HxVector 
operator*(const HxVector &a, const HxTMatrix<C> &b)
{
    if (a.nElem() != b.nRow()) {
        error("nonconformant HxVector * HxTMatrix<C> operands.");
        return HxVector(0);
    }
    HxVector v(b.nCol());
    double sum;
    int i, j;
    for (i=0 ; i<b.nCol() ; i++) {
        sum = 0;
        for (j=0 ; j<b.nRow() ; j++)
            sum += a[j] * b[j][i];
        v[i] = sum;
    }
    return v;
}


template<class C> HxVector 
operator*(const HxTMatrix<C> &a, const HxVector &b)
{
    if (b.nElem() != a.nCol()) {
        error("nonconformant HxTMatrix<C> * HxVector operands.");
        return HxVector(0);
    }
    HxVector v(a.nRow());
    double sum;
    int i, j;
    for (i=0 ; i<a.nRow() ; i++) {
        sum = 0;
        for (j=0 ; j<a.nCol() ; j++)
            sum += a[i][j] * b[j];
        v[i] = sum;
    }
    return v;
}


/*

template<class C> HxVector 
operator*(const HxTMatrix<C> &b, const HxVector &a)  
{
    if (a.nElem() != b.nCol()) {
        error("nonconformant HxTMatrix<C> * HxVector operands.");
        return HxVector(0);
    }
    HxVector v(b.nRow());
    double sum;
    int i, j;
    for (i=0 ; i<b.nRow() ; i++) {
        sum = 0;
        for (j=0 ; j<b.nCol() ; j++)
            sum += b[i][j] * a[j];
        v[i] = sum;
    }
    return v;
}

*/

template<class C> HxVec3Double 
operator*(const HxVec3Double &a, const HxTMatrix<C> &b)
{
    return b*a;
}



template<class C> HxVec3Double 
HxTMatrix<C>::operator*(const HxVec3Double &b) const
{
    if ((nRow() != 3) || (nCol() != 3)) {
        error("nonconformant HxTMatrix<C> * HxVec3Double operands.");
        return HxVec3Double();
    }
    double v[3];
    for (int i=0 ; i<nRow() ; i++) {
        v[i] = (*this)[i][0]*b.x() + (*this)[i][1]*b.y() + (*this)[i][2]*b.z();
    }
    return HxVec3Double(v[0], v[1], v[2]);
}

//*/

//---------------------------------------------------
//      Operations:
//---------------------------------------------------

template<class C>  void  
HxTMatrix<C>::write2disk(const HxString fileName, bool bin) const 
{
    FILE *fp;

    if(bin) {   // write to binary file
    
        fp = fopen(fileName.c_str(), "wb") ;

        //write the header of the matrix
        fwrite(&_nr, sizeof(int), 1, fp) ;
        fwrite(&_nc, sizeof(int), 1, fp) ;

        // write data
        fwrite(_data,sizeof(C),_nr*_nc,fp) ;

    }
    else {      // write to text file

        fp = fopen(fileName.c_str(), "w") ;

        //write the header of the matrix
        fprintf(fp,"%d \t %d \n", _nr, _nc) ;
        for(int i=0; i<_nr;i++) {
            for(int j=0; j<_nc;j++)
                fprintf(fp," %f",(double)(*this)[i][j] );
                fprintf(fp,"\n");
        }

    }

    fclose(fp) ;
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::i() const
{
    if (nRow() != nCol()) {
        error("Inverse: matrix is not square!.");
        return HxTMatrix<C>(0, 0);
    }

    int size = nRow();
    HxTMatrix<C> m(size,size), tmp(*this);
    short* idx = new short [size];
    double d;

    if (!ludcmp(tmp._data, size, idx, &d)) {
        error( "Inverse: singular matrix can't be inverted." );
        delete [] idx;
        return HxTMatrix<C>(0,0);
    }
    
    double * res = new double [size];

    for (int j = 0; j < size; j++) {
        for (int i = 0; i < size; i++)
            res[i] = 0.0;
        res[j] = 1.0;
        lubksb(tmp._data, size, idx, res);
        for (i = 0; i < size; i++)
            m[i][j] = res[i];
    }

     delete [] res;
     delete [] idx;

     return m;
}



template<class C> HxTMatrix<C> 
HxTMatrix<C>::svd(HxVector& W, HxTMatrix<C>& V) const
{
    int row = nRow();
    int col = nCol();

    if (col > row) {
        error( "Svd: HxTMatrix must be augmented with extra rows of zeros." );
        W = HxVector(0);
        V = HxTMatrix<C>(0,0);
        return HxTMatrix<C>(0,0);
    }

    HxTMatrix<C> U(*this);
    W = HxVector(col);
    V = HxTMatrix<C>(col,col);

    svdcmp(U._data, W, V._data, col, row);

    /* sort on eigenvalue */
    for (int i = 0; i < col; i++) {
        int idx = i;
        double val = W[idx];

        for (int j = i+1; j < col; j++)
            if( W[j] > val ) {
                idx = j;
                val = W[idx];
            }

        if (idx != i) {
            val = W[idx]; W[idx] = W[i]; W[i] = val;
            for (int j = 0; j < col; j++) {
                val = V[j][idx];
                V[j][idx] = V[j][i];
                V[j][i] = val;
            }
            for (j = 0; j < row; j++) {
                val = U[j][idx];
                U[j][idx] = U[j][i];
                U[j][i] = val;
            }
        }
    }

    return U;
}


template<class C> HxTMatrix<C>  
HxTMatrix<C>::app(const HxTMatrix<C>& b) const
{
       int newRows = b.nRow() + nRow() ;
       int newCols = b.nCol()>nCol() ? b.nCol() : nCol() ;
       int i,j ;

       HxTMatrix<C> m( newRows, newCols, (C) 0.0) ;

       for (i=0; i<nRow(); i++)
        for (j=0; j<nCol(); j++)
          m[i][j] = (*this)[i][j] ;

        for (i=nRow(); i<newRows; i++)
         for (j=0; j<b.nCol(); j++)
          m[i][j] = b[i-nRow()][j] ;

        return m;
}



template<class C> HxTMatrix<C> 
HxTMatrix<C>::add(const HxTMatrix<C>& b) const
{
    return *this+b;
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::add(const double val) const
{
    return *this+val;
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::sub(const HxTMatrix<C>& b) const
{
    return *this-b;
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::sub(const double val) const
{
    return *this-val;
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::mul(const HxTMatrix<C>& b) const
{
    return *this*b;
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::mul(const double val) const
{
    return *this*val;
}


template<class C> HxTMatrix<C> 
HxTMatrix<C>::mul(const HxVector& v) const
{
    return *this*v;
}


template<class C> HxTMatrix<C> 
HxTMatrix<C>::div(const double val) const
{
    return *this/val;
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::sin() const
{
    return conv<C>(conv<double>(*this).map(::sin));
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::cos() const
{
    return conv<C>(conv<double>(*this).map(::cos));
    //return map(::cos);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::tan() const
{
    return conv<C>(conv<double>(*this).map(::tan));
    //    return map(::tan);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::sinh() const
{
    return conv<C>(conv<double>(*this).map(::sinh));
//    return map(::sinh);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::cosh() const
{
    return conv<C>(conv<double>(*this).map(::cosh));
//    return map(::cosh);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::tanh() const
{
    return conv<C>(conv<double>(*this).map(::tanh));
//    return map(::tanh);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::exp() const
{
    return conv<C>(conv<double>(*this).map(::exp));
//    return map(::exp);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::log() const
{
    return conv<C>(conv<double>(*this).map(::log));
//  return map(::log);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::sqrt() const
{
    return conv<C>(conv<double>(*this).map(::sqrt));
//    return map(::sqrt);
}

template<class C> HxTMatrix<C> 
HxTMatrix<C>::abs() const
{
    return conv<C>(conv<double>(*this).map(::fabs));
//    return map(::fabs);
}

template<class C> 
inline C sgn(C a) { return a < 0.0 ? -1.0 : +1.0; }

template<class C> HxTMatrix<C> 
HxTMatrix<C>::sgn() const
{
    return map(::sgn);
}


//---------------------------------------------------
//      Larger Operations:
//---------------------------------------------------


// ===================== singular value decomposition =====================

#ifndef PI
#define PI M_PI
#endif

/*
PYTHAG computes sqrt(a^{2} + b^{2}) without destructive overflow or underflow.
*/
static double at, bt, ct;
#define PYTHAG(a, b) ((at = fabs(a)) > (bt = fabs(b)) ? \
(ct = bt/at, at*sqrt(1.0+ct*ct)): (bt ? (ct = at/bt, bt*sqrt(1.0+ct*ct)): 0.0))

static double maxarg1, maxarg2;
#define MAX(a, b) (maxarg1 = (a), maxarg2 = (b), (maxarg1) > (maxarg2) ? \
(maxarg1) : (maxarg2))

static int imaxarg1, imaxarg2;
#define IMIN(a,b)    (imaxarg1=(a),imaxarg2=(b),imaxarg1 > imaxarg2 ? \
    imaxarg2 : imaxarg1)

#define SIGN(a, b) ((b) < 0.0 ? -fabs(a): fabs(a))


/*
Given a matrix a[m][n], this routine computes its singular value
decomposition, A = U*W*V^{T}.  The matrix U replaces a on output.
The diagonal matrix of singular values W is output as a vector w[n].
The matrix V (not the transpose V^{T}) is output as v[n][n].
m must be greater or equal to n;  if it is smaller, then a should be
filled up to square with zero rows.
*/

template<class C> 
static void svdcmp(C* a, HxVector &w, C* v, int n, int m)
{
    double *rv1 = new double [n];
    double g = 0.0;
    double scale = 0.0;
    double anorm = 0.0;
    int l = 0;

    for (int i=0;i<n;i++) {
//      cout << "Don't KILL me yet.. still doing something, " << i << " till " << n << endl;
        l=i+1;
        rv1[i]=scale*g;
        g = 0.0;
        scale = 0.0;

        if (i < m) {
            for (int k=i;k<m;k++)
                scale += fabs(a[k*n+i]);
            if (scale) {
                double s = 0.0;
                for (int k=i;k<m;k++) {
                    a[k*n+i] /= scale;
                    s += a[k*n+i]*a[k*n+i];
                }

                double f=a[i*n+i];
                g = -SIGN(sqrt(s),f);

                double h=f*g-s;
                a[i*n+i]=f-g;

                for (int j=l;j<n;j++) {
                    s = 0.0;
                    for (int k=i;k<m;k++)
                        s += a[k*n+i]*a[k*n+j];
                    f=s/h;
                    for (k=i;k<m;k++)
                        a[k*n+j] += f*a[k*n+i];
                }
                for (k=i;k<m;k++)
                    a[k*n+i] *= scale;
            }
        }
        w[i]=scale*g;
        g = 0.0;
        scale = 0.0;
        if (i < m && i != n-1) {
            for (int k=l;k<n;k++)
                scale += fabs(a[i*n+k]);
            if (scale) {
                double s = 0.0;

                for (int k=l;k<n;k++) {
                    a[i*n+k] /= scale;
                    s += a[i*n+k]*a[i*n+k];
                }

                double f=a[i*n+l];
                g = -SIGN(sqrt(s),f);

                double h=f*g-s;
                a[i*n+l]=f-g;

                for (k=l;k<n;k++)
                    rv1[k]=a[i*n+k]/h;
                for (int j=l;j<m;j++) {
                    s = 0.0;
                    for (int k=l;k<n;k++)
                        s += a[j*n+k]*a[i*n+k];
                    for (k=l;k<n;k++)
                        a[j*n+k] += s*rv1[k];
                }
                for (k=l;k<n;k++)
                    a[i*n+k] *= scale;
            }
        }
        anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
    }
    for (i=n-1;i>=0;i--) {
        if (i < n-1) {
            if (g) {
                for (int j=l;j<n;j++)
                    v[j*n+i]=(a[i*n+j]/a[i*n+l])/g;

                for (j=l;j<n;j++) {
                    double s = 0.0;
                    for (int k=l;k<n;k++)
                        s += a[i*n+k]*v[k*n+j];
                    for (k=l;k<n;k++)
                        v[k*n+j] += s*v[k*n+i];
                }
            }
            for (int j=l;j<n;j++)
                v[i*n+j]=v[j*n+i]=0.0;
        }
        v[i*n+i]=1.0;
        g=rv1[i];
        l=i;
    }
    for (i=IMIN(m,n)-1;i>=0;i--) {
        l=i+1;
        g=w[i];
        for (int j=l;j<n;j++)
            a[i*n+j]=0.0;
        if (g) {
            g=1.0/g;
            for (int j=l;j<n;j++) {
                double s = 0.0;
                for (int k=l;k<m;k++)
                    s += a[k*n+i]*a[k*n+j];

                double f=(s/a[i*n+i])*g;
                for (k=i;k<m;k++)
                    a[k*n+j] += f*a[k*n+i];
            }
            for (j=i;j<m;j++)
                a[j*n+i] *= g;
        }
        else
            for (int j=i;j<m;j++)
                a[j*n+i]=0.0;
        a[i*n+i]++;
    }

    for (int k=n-1;k>=0;k--) {
        for (int its=1;its<=30;its++) {
            int flag=1;
            int nm;
            double f, h, x, y, z;

            for (l=k;l>=0;l--) {
                nm=l-1;
                if ((double)(fabs(rv1[l])+anorm) == anorm) {
                    flag=0;
                    break;
                }
                if ((double)(fabs(w[nm])+anorm) == anorm) break;
            }

            if (flag) {
                double c = 0.0;
                double s = 1.0;

                for (int i=l;i<=k;i++) {
                    f=s*rv1[i];
                    rv1[i]=c*rv1[i];
                    if ((double)(fabs(f)+anorm) == anorm) break;
                    g=w[i];
                    h=PYTHAG(f,g);
                    w[i]=h;
                    h=1.0/h;
                    c=g*h;
                    s = -f*h;
                    for (int j=0;j<m;j++) {
                        y=a[j*n+nm];
                        z=a[j*n+i];
                        a[j*n+nm]=y*c+z*s;
                        a[j*n+i]=z*c-y*s;
                    }
                }
            }

            z=w[k];
            if (l == k) {
                if (z < 0.0) {
                    w[k] = -z;
                    for (int j=0;j<n;j++) v[j*n+k] = -v[j*n+k];
                }
                break;
            }
            if (its == 30) {
                delete [] rv1;
                error( "HxMatrix: no convergence in 30 svdcmp iterations");
            }
            x=w[l];
            nm=k-1;
            y=w[nm];
            g=rv1[nm];
            h=rv1[k];
            f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
            g=PYTHAG(f,1.0);
            f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;

            double c = 1.0;
            double s = 1.0;
            for (int j=l;j<=nm;j++) {
                int i=j+1;
                g=rv1[i];
                y=w[i];
                h=s*g;
                g=c*g;
                z=PYTHAG(f,h);
                rv1[j]=z;
                c=f/z;
                s=h/z;
                f=x*c+g*s;
                g=g*c-x*s;
                h=y*s;
                y *= c;
                for (int jj=0;jj<n;jj++) {
                    x=v[jj*n+j];
                    z=v[jj*n+i];
                    v[jj*n+j]=x*c+z*s;
                    v[jj*n+i]=z*c-x*s;
                }
                z=PYTHAG(f,h);
                w[j]=z;
                if (z) {
                    z=1.0/z;
                    c=f*z;
                    s=h*z;
                }
                f=c*g+s*y;
                x=c*y-s*g;
                for (jj=0;jj<m;jj++) {
                    y=a[jj*n+j];
                    z=a[jj*n+i];
                    a[jj*n+j]=y*c+z*s;
                    a[jj*n+i]=z*c-y*s;
                }
            }
            rv1[l]=0.0;
            rv1[k]=f;
            w[k]=x;
        }
    }

    delete [] rv1;
}



// ===================== matrix inverse =====================

template<class C> 
static int ludcmp(C* a, int n, short *indx, double *d)
{
    double *vv = new double [n];

    *d=1.0;
    for (int i=0;i<n;i++) {
        double big=0.0;
        for (int j=0;j<n;j++) {
            double temp = fabs(a[i*n+j]);
            if (temp > big)
                big=temp;
        }
        if (big == 0.0)
            return(0); // Singular matrix 
        vv[i]=1.0/big;
    }
    for (int j=0;j<n;j++) {
        for (int i=0;i<j;i++) {
            double sum=a[i*n+j];
            for (int k=0;k<i;k++)
                sum -= a[i*n+k]*a[k*n+j];
            a[i*n+j]=sum;
        }
        double big=0.0;
        int imax = 0;
        for (i=j;i<n;i++) {
            double sum=a[i*n+j];
            for (int k=0;k<j;k++)
                sum -= a[i*n+k]*a[k*n+j];
            a[i*n+j]=sum;

            double dum = vv[i]*fabs(sum);
            if ( dum >= big) {
                big=dum;
                imax=i;
            }
        }
        if (j != imax) {
            for (int k=0;k<n;k++) {
                double dum=a[imax*n+k];
                a[imax*n+k]=a[j*n+k];
                a[j*n+k]=dum;
            }
            *d = -(*d);
            vv[imax]=vv[j];
        }
        indx[j]=imax;
        if (a[j*n+j] == 0.0)
            a[j*n+j]=HxMatrix_EPS;
        if (j != n-1) {
            double dum=1.0/a[j*n+j];
            for (int i=j+1;i<n;i++)
                a[i*n+j] *= dum;
        }
    }
    delete [] vv;

    return(1);
}

template<class C> 
static void lubksb(C* a, int n, short *indx, double *b)
{
    int ii = -1;

    for (int i=0;i<n;i++) {
        int ip=indx[i];
        double sum=b[ip];
        b[ip]=b[i];
        if (ii>=0)
            for (int j=ii;j<=i-1;j++)
                sum -= a[i*n+j]*b[j];
        else if (sum) ii=i;
        b[i]=sum;
    }
    for (i=n-1;i>=0;i--) {
        double sum=b[i];
        for (int j=i+1;j<n;j++)
            sum -= a[i*n+j]*b[j];
        b[i]=sum/a[i*n+i];
    }
}


// ===================== K-means clustering =====================

/*
 *  K-means clustering algorithm
 *  Author(s):
 *  Jan van Gemert (jvgemert@science.uva.nl)
 *  Hieu T. Nguyen (tat@science.uva.nl)
 */

/*==============================================================================*/

// calculate vec distance
template<class C>
inline double vec_distance(C* v1, C* v2, int nElem) 
{
    double s=0,tmp ;
    for(int i=0;i<nElem;i++) 
        s += (tmp = v1[i] - v2[i],tmp*tmp);
    return sqrt(s);
}


// calculate vec distance, given a current minimum
template<class C>
inline double vec_distance(C* v1, C* v2, int nElem, C currentMin) 
{
    if(currentMin>0) {
        C currentMinSquared = currentMin*currentMin ;
        double s=0,tmp ;
        int i=0 ;
        while(s<currentMinSquared && i<nElem) {
                s += (tmp = v1[i] - v2[i],tmp*tmp);
                i++;
        }
        return i==nElem?sqrt(s):currentMin ;
    }
    return currentMin ;

    
}


/*==============================================================================*/

#include<list>

template<class C> void
HxTMatrix<C>::kmeans(HxTMatrix<C> &c, HxVector &imap, int numloop, bool cIsInitialized) const 
{
    int i,j;

    int nrVectors = _nr ; 
    int nrDimensions = _nc ;
    int nrClusters = c.nRow() ;
    //printf("K-means, nrClusters= %d, nr=%d, nc=%d\n", nrClusters, _nr, _nc );

    imap = HxVector(nrVectors) ;
    if(!cIsInitialized) {
            // the cluster matrix is not initialized, initialize it with some vectors.
        c = HxTMatrix<C>(nrClusters,nrDimensions) ;
        int steps = nrVectors / nrClusters ;
        for(i=0;i<nrClusters;i++) {
            for(j=0;j<nrDimensions;j++) {
                    c[i][j] = (*this)[i*steps][j] ;
            }

//          printf("cluster %d in data %d \n", i, i*steps);

                /* start of code to check if this vector did not occur already
            int k=0; 
            bool occurs = false ;
            int index = i * _nc ;
            while(k<i && !occurs) {

                double d = vec_distance(&c._data[k*_nc], &c._data[index], _nc, 0.01); 
                if(d < 0.01) occurs = true ;
                else k++ ;
            }

            if(occurs) printf("dist between %d and %d Small !\n",i,k) ;
            //*/
        }
    } 

    // check c matrix dimensions
    if(c.nCol() != nrDimensions) {
        error( "error kmeans: c matrix does not have similar dimensionality." );
        exit(1);
    }
    
    std::list<int> *mylist;
    std::list<int>::const_iterator listIter;
    bool contin = true;
    double err = 0, errOld=0;

    int emptyclust =0 ;
    while(contin)
    {
        emptyclust =0 ;
        err=0 ;
        contin = false;
        // create a list for each cluster vector, and keep the data vectors who 
        // belong to a cluster in their corresponding list.
        mylist = new std::list<int>[nrClusters];
        for(i=0;i<nrVectors;i++)
        {
            int jm=0;
            double d = 0;
            int indexI = i*_nc ;
            double dm = vec_distance(&_data[indexI],c._data, _nc);
            for(j=1;j<nrClusters;j++) {
                d = vec_distance(&_data[indexI], &c._data[j*_nc], _nc, dm); 
                if( d<dm ) { dm = d;jm=j; }
            }
//          std::cout << i << " in " << jm << std::endl ;           
            mylist[jm].push_back(i);
            err += dm;
        }
        
        numloop--;
//      printf("recalculate\n");
        // recalculate the position of the cluster vectors, 
        // based on the datavectors who belong to the cluster
        for(i=0;i<nrClusters;i++) {
            if( (mylist[i].size()) > 0 )
            {
                for(j=0;j<nrDimensions;j++) c[i][j] = 0;

                std::list<int>::const_iterator myListEnd = mylist[i].end() ;
                for(listIter = mylist[i].begin(); listIter!=myListEnd;listIter++)
                    for(j=0;j<nrDimensions;j++) 
                        c[i][j] += (*this)[*listIter][j];

                for(j=0;j<nrDimensions;j++) c[i][j] /= mylist[i].size();
            }
            else { 
                emptyclust++ ;
                printf("empty cluster: %d\n",i);
            }
        }

        //printf("loop %d dist=%f, distOld=%f, emptyClusters: %d\n",numloop, err, errOld, emptyclust);
//      printf("%d, ",numloop);
        // quit if the numLoops is reached or if the error stays constant
        if(err != errOld && numloop>0) contin = true;

        errOld = err;

    } // end while(contin)

    printf("\n");
    // for each data vector, store the cluster it belongs to in vector imap
    int filledClusters = 0 ;
    for(i=0;i<nrClusters;i++) 
        if(!mylist[i].empty())  {
            for(listIter= mylist[i].begin();listIter!=mylist[i].end();listIter++)
                imap[*listIter] = filledClusters; 
            filledClusters++ ;
        }

    // if there are empty clusters, remove them.
    if(emptyclust > 0 ) {
        printf("removing empty clusters: new clustersize = %d = %d",nrClusters-emptyclust,filledClusters);  
        int counter=0 ;
        HxTMatrix<C> tmpC(nrClusters-emptyclust, nrDimensions) ;
    
        // copy filled clusters to a temp. matrix
        for(i=0;i<nrClusters;i++) 
            if(!mylist[i].empty()) { 
                for(j=0;j<nrDimensions;j++) tmpC[counter][j] = c[i][j];
                counter++ ;
            }
        c = tmpC ;
    }

    delete[] mylist;

}


// ===================== covariance matrix =====================
/*
 *  covariance matrix
 *  Author(s):
 *  Minh A. Hoang (minh@science.uva.nl)
 *  Jan van Gemert (jvgemert@science.uva.nl)
 * Compute the covariance matrix of the observed data
 * Output: [nc x nc] covariance matrix and a vector of mean-values for each dimension.
 */

template<class C> HxTMatrix<C> 
HxTMatrix<C>::covariance(HxVector &mean) const 
{
//  printf("Calculating the covariance matrix...");
    int i,j,k;

    HxTMatrix<C> COVxy(_nc,_nc, 0.0) ;

    mean = HxVector(_nc) ; //means
    for (i=0; i<_nc; i++) mean[i] = 0;

    for (k=0; k<_nr; k++)
        for (i=0; i<_nc; i++) {
            mean[i] += (*this)[k][i];
            for (j=i; j<_nc; j++)
                COVxy[i][j] += ((*this)[k][i] * (*this)[k][j]) ;
        }

    for (i=0; i<_nc; i++) mean[i] /= _nr;

    for (i=0; i<_nc; i++)
        for (j=i; j<_nc; j++) {
            COVxy[i][j] = COVxy[i][j]/_nr - mean[i]*mean[j];
            COVxy[j][i] = COVxy[i][j];
        }

//  printf("...done\n");
    return COVxy;
}


// ===================== Karhunen Loeve mapping (PCA) =====================
/*
 *  Karhunen Loeve mapping (PCA)
 *  Author(s):
 *  Minh A. Hoang (minh@science.uva.nl)
 *  Jan van Gemert (jvgemert@science.uva.nl)
 *  Principal Componant Analysis to reduce matrix to newDim dimensions
 */

// KLM helping functions

/*
 Householder reduction of a real, symmetric matrix a[0..n-1][0..n-1]
 Output: a is replaced by the orthogonal matrix Q effecting the
 transformation. d[0..n-1] returns the diagonal elements of the
 tridiagonal matrix, and e[0..n-1] the off-diagonal elements, with e[0]=0;
 */
template<class C> 
void tred2(HxTMatrix<C> &a, HxVector &d, HxVector &e)
{
    int i,j,k,l;
    double scale,hh,h,g,f;

    int n = d.nElem() ;

    for (i=n-1; i>0; i--)
    {
        l = i-1;
        h = scale = 0.0;
        if (l>0)
        {
            for (k=0; k<=l; k++) scale += fabs(a[i][k]);
            if (scale == 0.0) e[i] = a[i][l];
            else
            {
                for (k=0; k<=l; k++)
                {
                    a[i][k] /= scale; //use scale a's for transformation
                    h += a[i][k]*a[i][k];
                }
                f = a[i][l];
                g = (f >=0.0 ? -sqrt(h) : sqrt(h));
                e[i] = scale*g;
                h -= f*g;
                a[i][l] = f-g;
                f = 0.0;
                for (j=0; j<=l; j++)
                {
                    a[j][i] = a[i][j]/h;
                    g = 0.0;
                    for (k=0; k<=j; k++) g += a[j][k]*a[i][k];
                    for (k=j+1; k<=l; k++) g += a[k][j]*a[i][k];
                    e[j] = g/h; //form element of p in temporarily unused element of e
                    f += e[j]*a[i][j];
                }
                hh = f/(h+h);
                for (j=0; j<=l; j++)
                {
                    f = a[i][j];
                    e[j] = g = e[j]-hh*f;
                    for (k=0; k<=j; k++) a[j][k] -= (f*e[k]+g*a[i][k]);
                }
            }
        }
        else e[i] = a[i][l];
        d[i] = h;
    }

    d[0] = 0.0;
    e[0] = 0.0;

    for (i=0; i<n; i++)
    {
        l = i-1;
        if (d[i])
        {
            for (j=0; j<=l; j++)
            {
                g = 0.0;
                for (k=0; k<=l; k++) g += a[i][k]*a[k][j];
                for (k=0; k<=l; k++) a[k][j] -= g*a[k][i];
            }
        }
        d[i] = a[i][i];
        a[i][i] = 1.0;
        for (j=0; j<=l; j++) a[j][i] = a[i][j] = 0.0;
    }

}

/*
 QL algorithm with implicit shifts, to determin the eigenvalues and eigenvectors
 of a real, symetric, tridiagonal matrix, of of a real, symmetric matrix previously
 reduced by tred2
 Input: d[0..n-1] contains the diagonal elements of the tridiagonal matrix
 e[0..n-1] inputs the subdiagonal elements of the tridiagonal matrix with e[0] arbitrary
 Output: d returns the eigenvalues, e is destroyed.
 If the eigenvectors of a tridiagonal matrix are desired, the matrix z[0..n-1][0..n-1]
 is input as the identity matrix. If the eigenvectors of a matrix that has been reduced
 by tred2 are required, then z is input as the matrix output by tred2.
 In either case, the kth column of z returns the normalized eigenvector corresponding to d[k]
 */
template<class C> 
void tqli(HxVector &d, HxVector &e, HxTMatrix<C> &z)
{

    int m,l,iter,i,k;
    double s,r,p,g,f,dd,c,b;

    int n = e.nElem() ;

    for (i=1; i<n; i++) e[i-1]=e[i]; //renumber
    e[n-1] = 0.0;


    for (l=0; l<n; l++)
    {
        iter=0;
        do
        {
            for (m=l; m<n-1; m++)
            {
                dd = fabs(d[m]) + fabs(d[m+1]);
                if ( (fabs(e[m]) + dd) == dd) break;
            }
            if (m != l)
            {
                if (iter++ == 30) printf("Too many iterations in tqli\n");
                g = (d[l+1]-d[l])/(2.0*e[l]);
                r = sqrt(g*g + 1.0);//=pythag(g,1.0);
                g = d[m] - d[l] + e[l]/(g+SIGN(r,g));
                s = c = 1.0;
                p = 0.0;
                for (i=m-1; i>=l; i--)
                {
                    f = s*e[i];
                    b = c*e[i];
                    e[i+1] = (r=sqrt(f*f+g*g));//=(r=pythag(f,g));
                    if (r == 0.0)
                    {
                        d[i+1] -= p;
                        e[m] = 0.0;
                        break;
                    }
                    s = f/r;
                    c = g/r;
                    g = d[i+1]-p;
                    r = (d[i]-g)*s + 2.0*c*b;
                    d[i+1] = g+(p=s*r);
                    g = c*r-b;

                    for (k=0; k<n; k++)
                    {
                        f = z[k][i+1];
                        z[k][i+1] = s*z[k][i] + c*f;
                        z[k][i] = c*z[k][i] - s*f;
                    }
                }
                if (r==0.0 && i>=l) continue;
                d[l] -= p;
                e[l] = g;
                e[m] = 0.0;
            }
        } while (m != l);
    }

    //eigenvalues d[0..n-1], eigenvectors z[0..n-1][0..n-1], sort them now
    int j;
    for (i=0; i<n-1; i++)
    {
        p = d[k=i];
        for (j=i+1; j<n; j++) if (d[j]>=p) p = d[k=j];
        if (k!=i) { //insert
            d[k] = d[i];
            d[i] = p;
            //exchange corresponding eigenvectors
            for (j=0; j<n; j++)
            {
                p = z[j][i];
                z[j][i] = z[j][k];
                z[j][k] = p;
            }
        }
    }
}


//  Karhunen Loeve mapping (PCA)


template<class C> HxTMatrix<C> 
HxTMatrix<C>::klm(int newDim) const
{
    if (newDim<1 || newDim>_nc)
    {
        printf("klm: new dimensionality is invalid, dimsize is set as 1...\n");
        newDim = 1;
    }

    HxTMatrix<C> newfvec(_nr,newDim);

    HxTMatrix<C> a = this->covariance() ;

    HxVector d(_nc) ;

    HxVector e(_nc) ;

    tred2(a,d,e);

    tqli(d,e,a);

    // d[k] is an eigenvalue and a[][k] is a corresponding eigenvector

    for (int k=0; k<_nr; k++)
        for (int i=0; i<newDim; i++) {
            double vl = 0.0;
            for (int j=0; j<_nc; j++) vl += ((*this)[k][j]*a[j][i]);
            newfvec[k][i] = vl;
        }

    return newfvec;
}



#endif

---