Matrix.h

Go to the documentation of this file.

#pragma once

#include "standard.h"

#pragma warning( push, 3 )
#include <numeric> // inner_product
#include <xutility> // swap
#pragma warning ( pop )

#include <malloc.h> // alloca
#include <assert.h>

#ifndef COMPILETIME_ASSERT
#define COMPILETIME_ASSERT(assertion) do { if (0) { char ch[(assertion)] = { 'a' }; ch;} } while (false)
#endif

// COMPILETIME_ASSERT gives us meaningless warnings unless we disable them
#pragma warning ( disable:4127 ) // conditional expression is constant


// -------------------------------------------------------------------
// The two types of matrices exposed by these templates:

template < int cols, int rows, typename T > 
class Matrix; 

template< int size, typename T >
class SymmetricMatrix; 

template< int height, typename T >
class ColVector;


template< int size, typename T >
class DataBlock
{
    protected:

        inline DataBlock() {}
            inline DataBlock<size,T> add(const DataBlock< size, T > &d) const
            {
                    DataBlock<size,T> rtn;

                    for (int i = 0; i < size; i++)
                            rtn.array[i] = array[i] + d.array[i];

                    return rtn;
            }

            inline void addToMe(const DataBlock< size, T> &d)
            {
                    for (int i = 0; i < size; i++)
                            array[i] = array[i] + d.array[i];
            }

            inline void subtractFromMe(const DataBlock< size, T> &d)
            {
                    for (int i = 0; i < size; i++)
                            array[i] = array[i] - d[i];
            }

            inline DataBlock<size,T> subtract (const DataBlock< size, T> &d) const
            {
                    DataBlock<size,T> rtn;
                    for (int i = 0; i < size; i++)
                            rtn.array[i] = array[i] - d.array[i];               

                    return rtn;
            }

            inline bool isEqualTo (const DataBlock< size, T> &d) const
            {
                    return (memcmp (array, d.array, sizeof(array)) == 0);
            }

            inline void copyFrom (const DataBlock<size, T> &d)
            {
                    memcpy (&array[0], &d.array[0], sizeof(array));
            }

            /* multiplication methods for ints, floats, and doubles */

            inline DataBlock<size,T> multiply (int n) const
            {
                    DataBlock<size,T> rtn;
                    for (int i = 0; i < size; i++)
                            rtn.array[i] = array[i] * n;                

                    return rtn;
            }

            inline DataBlock<size,T> multiply (float n) const
            {
                    DataBlock<size,T> rtn;
                    for (int i = 0; i < size; i++)
                            rtn.array[i] = array[i] * n;                

                    return rtn;
            }


            inline DataBlock<size,T> multiply (double n) const
            {
                    DataBlock<size,T> rtn;
                    for (int i = 0; i < size; i++)
                            rtn.array[i] = (T)(array[i] * n);           

                    return rtn;
            }

            /* division methods for ints, floats, and doubles */

            inline DataBlock<size,T> divide (int n) const
            {
                    DataBlock<size,T> rtn;
            T inverse = 1. / n;

                    for (int i = 0; i < size; i++)
                            rtn.array[i] = array[i] * inverse;          

                    return rtn;
            }

            inline DataBlock<size,T> divide (float n) const
            {
                    DataBlock<size,T> rtn;
            T inverse = 1. / n;

                    for (int i = 0; i < size; i++)
                            rtn.array[i] = array[i] * inverse;          

                    return rtn;
            }

            inline DataBlock<size,T> divide (double n) const
            {
                    DataBlock<size,T> rtn;

            T inverse = (T)(1. / n);

                    for (int i = 0; i < size; i++)
                            rtn.array[i] = array[i] * inverse;          

                    return rtn;
            }


            T array[size];
    public:

            inline T * getData()
            { return array; }
            
            inline setToZero()
            {
                    memset(array, 0, sizeof(array));
            }
};

#define TRIANGULAR(i) ( ((i + 1)*i) / 2)

template< int size, typename T >
class SymmetricMatrix : public DataBlock< TRIANGULAR(size), T >
{
    private:
            typedef SymmetricMatrix< size, T > _Myt;
            typedef DataBlock< TRIANGULAR(size), T > parentType;
            typedef Matrix< size, size, T > MtxAsymmetric;

    public:

            SymmetricMatrix() {}
            SymmetricMatrix(const parentType &p) {copyFrom (p);}
            SymmetricMatrix(const MtxAsymmetric &m)
            {
                    this->operator=(m);
            }
            const enum {nColumns = size};
            const enum {nRows = size};

            //---------------------------------------------

            _Myt operator-(const _Myt &m)  const { return this->subtract(m);}
            _Myt operator+(const _Myt &m)  const { return this->add(m);}
            _Myt operator*(int n)          const { return this->multiply(n); }
            _Myt operator*(float f)        const { return this->multiply(f); }
            _Myt operator*(double f)       const { return this->multiply(f); }
            _Myt operator/(int n)          const { return this->divide(n); }
            _Myt operator/(float n)        const { return this->divide(n); }
            _Myt operator/(double n)       const { return this->divide(n); }
            bool operator==(_Myt &m)       const { return this->isEqualTo (m); }
            void operator+=(_Myt &m)      { this->addToMe (m); }
            void operator-=(_Myt &m)      { this->subtractFromMe (m); }
            void operator=(const _Myt &m) { this->copyFrom (m); }
            inline T &  operator()(int col, int row) { return elementAt(col,row);}
        inline T &  operator[](int n) { return array[n]; }
        inline T   operator[](int n) const { return array[n]; }
            void print(char *str = "") const { printMatrix(*this,str); } 

            template <int N>
            Matrix< N, nRows, T> operator* (const SymmetricMatrix<N, T> &m) const
            {
                    
                    // If it fails to compile here, that means that you've tried to 
                    // multiply two matrices whose sizes don't match. The first
                    // Matrix should have as many columns as the second Matrix's rows
            COMPILETIME_ASSERT (cols == N);

            return multMatrix< _Myt, Matrix< N, N, T >, T, rows, N>(*this, m);
            }

            template <int theirCols, int theirRows>
            Matrix< theirCols, nRows, T> operator* (const Matrix< theirCols, theirRows, T> &m) const
            {
                    
                    // If it fails to compile here, that means that you've tried to 
                    // multiply two matrices whose sizes don't match. The first
                    // Matrix should have as many columns as the second Matrix's rows
            COMPILETIME_ASSERT (cols == theirRows);

            return multMatrix< _Myt, Matrix< theirCols, theirRows, T >, T, nRows, theirCols>(*this, m);
            }

            //---------------------------------------------

            void operator=(const Matrix<size, size, T> &m)
            {
                    for (int col = 0; col < nColumns; col++)
                            for (int row = col; row < nRows; row++)
                                    elementAt (col,row) = m.elementAt(col,row);
            }

            inline T & elementAt (int col, int row) 
            {
                    assert (col >= 0 && row >= 0 && col < nColumns && row < nRows);

                    return (col > row)?
                            array[row*nRows + col - TRIANGULAR(row)]: // swapped
                            array[col*nRows + row - TRIANGULAR(col)]; // not swapped
            }

            inline T elementAt (int col, int row) const
            {
                    assert (col >= 0 && row >= 0 && col < nColumns && row < nRows);

                    return (col > row)?
                            array[row*nRows + col - TRIANGULAR(row)]: // swapped
                            array[col*nRows + row - TRIANGULAR(col)]; // not swapped
            }
};

// -------------------------------------------------------------------
// Matrix: this class holds a regular matrix.  It supports most
// operations with...
//
//     * other "SymmetricMatrix" objects
//     * "Matrix" objects
//     * floats and doubles

template < int cols, int rows, typename T >
class Matrix : public DataBlock< cols*rows, T >
{
    private:
            typedef Matrix< cols, rows, T > _Myt;
            typedef DataBlock< cols*rows, T> parentType;
            typedef SymmetricMatrix< cols, T > MtxSymmetric;

    public:
            Matrix() {}
            Matrix(const parentType &p) {copyFrom (p);}
            Matrix(const MtxSymmetric &m)
            {
            COMPILETIME_ASSERT(nRows == nColumns);
                    this->operator=(m);
            }

            const enum {nColumns = cols};
            const enum {nRows = rows};

            //---------------------------------------------

            _Myt operator-(const _Myt &m)  const { return this->subtract(m);}
            _Myt operator+(const _Myt &m)  const { return this->add(m);}
            _Myt operator*(int n)          const { return this->multiply(n); }
            _Myt operator*(float f)        const { return this->multiply(f); }
            _Myt operator*(double f)       const { return this->multiply(f); }
            _Myt operator/(int n)          const { return this->divide(n); }
            _Myt operator/(float n)        const { return this->divide(n); }
            _Myt operator/(double n)       const { return this->divide(n); }
            bool operator==(_Myt &m)       const { return this->isEqualTo (m); }
            void operator+=(_Myt &m)      { this->addToMe (m); }
            void operator-=(_Myt &m)      { this->subtractFromMe (m); }
            void operator=(const _Myt &m) { this->copyFrom (m); }
            inline T &  operator()(int col, int row) { return elementAt(col,row);}
        inline T &  operator[](int n) { return array[n]; }
        inline T   operator[](int n) const { return array[n]; }
            void print(char *str = "") const { printMatrix(*this,str); } 

        template <int N>
            Matrix< N, nRows, T> operator* (const SymmetricMatrix<N, T> &m) const
            {
                    
                    // If it fails to compile here, that means that you've tried to 
                    // multiply two matrices whose sizes don't match. The first
                    // Matrix should have as many columns as the second Matrix's rows
            COMPILETIME_ASSERT (cols == N);

            return multMatrix< _Myt, Matrix< N, N, T >, T, rows, N>(*this, m);
            }

            template <int theirCols, int theirRows>
            Matrix< theirCols, nRows, T> operator* (const Matrix< theirCols, theirRows, T> &m) const
            {
                    
                    // If it fails to compile here, that means that you've tried to 
                    // multiply two matrices whose sizes don't match. The first
                    // Matrix should have as many columns as the second Matrix's rows
            COMPILETIME_ASSERT (cols==theirRows);

            return multMatrix< _Myt, Matrix< theirCols, theirRows, T >, T, nRows, theirCols>(*this, m);
            }
            //---------------------------------------------

            void operator=(const SymmetricMatrix<cols, T> &m)
            {
                    for (int col = 0; col < cols; col++)
                            for (int row = 0; row < rows; row++)
                                    elementAt (col,row) = m.elementAt (col,row);
            }



        T determinant()
        {
            COMPILETIME_ASSERT(nRows==nColumns);
            COMPILETIME_ASSERT(nRows>1);

            if (nRows == 1)
            {
                return 0;
            }

            if (nRows == 2)
            {
                return (elementAt(0,0) * elementAt(1,1) - 
                        elementAt(1,0) * elementAt(0,1));
            }

            T total = 0.;
            for (int i = 0; i < nColumns; i++)
            {
                Submatrix sub;
                sub = getSubmatrix(i, 0);

                T det = sub.determinant();

                if (0 == i % 2)
                    total += det;
                else
                    total -= det;
            }

            return total;
        }
        
        inline T & elementAt (int col, int row)
            {
                    assert (col >= 0 && row >= 0 && col < nColumns && row < nRows);

                    return array[col*rows + row];
            }

            inline T  elementAt (int col, int row) const
            {
                    assert (col >= 0 && row >= 0 && col < nColumns && row < nRows);

                    return array[col*rows + row];
            }

            Matrix <rows, cols, T> getTranspose() const
            {
                    Matrix<rows, cols, T> rtn;
                    for (int row = 0; row < rows; row++)
                            for (int col = 0; col < cols; col++)
                                    rtn(row,col) = elementAt(col,row);

                    return rtn;
            }


            void setToIdentity()
            {
            COMPILETIME_ASSERT(nRows==nColumns);
                    setToZero();
                    
                    for (int n = 0; n < cols; n++)
                            elementAt (n,n) = 1;
            }

            bool getInverse(Matrix<cols,rows,T> &answer) const
            {
            COMPILETIME_ASSERT(nRows==nColumns);
                    
                    Matrix<cols,rows,T> mtx;
                    mtx = *this;
                    answer.setToIdentity();
                    
                    // for each column...
                    for (int col = 0; col < nColumns; col++)
                    {
                            int rowMax = col;
                            
                            for (int row=col+1; row<nRows; row++)
                            {
                                    if (mtx(row,col) > mtx(rowMax, col))
                                            rowMax = row;
                            }
                            
                            if (mtx(rowMax, col) == 0) 
                            {
                                    return false;
                            }
                            
                            // Swap row "rowMax" with row "c"
                            for (int cc=0; cc<nColumns; cc++)
                            {
                                    T tmp = mtx(col,cc);
                                    mtx(col,cc) = mtx(rowMax,cc);
                                    mtx(rowMax,cc) = tmp;
                                    tmp = answer(col,cc);
                                    answer(col,cc) = answer(rowMax,cc);
                                    answer(rowMax,cc) = tmp;
                            }
                            
                            // Now everything we do is on row "c".
                            // Set the max cell to 1 by dividing the entire row by that value
                            T tmp = mtx(col,col);
                            for (cc=0; cc<nColumns; cc++)
                            {
                                    mtx(col,cc) =  mtx(col,cc) / tmp;
                                    answer(col,cc) = answer(col,cc) / tmp;
                            }
                            
                            // Now do the other rows, so that this column only has a 1 and 0's
                            for (row = 0; row < nRows; row++)
                            {
                                    if (row != col)
                                    {
                                            tmp = mtx (row,col);
                                            for (cc=0; cc<nColumns; cc++)
                                            {
                                                    mtx(row,cc) = mtx(row,cc) - mtx(col,cc) * tmp;
                                                    answer(row,cc) = answer(row,cc) - answer(col,cc)*tmp;
                                            }
                                    }
                            }
                            
                    }
                    return true;
            }

    private:
        typedef Matrix< (nColumns>2?nColumns-1:nColumns), (nRows>2?nRows-1:nRows), T > Submatrix;
        Submatrix getSubmatrix (int colToDelete, int rowToDelete)
        {
            COMPILETIME_ASSERT(nRows==nColumns);
            COMPILETIME_ASSERT(nRows>1);

            Submatrix rtn;

            int curSpot = 0;
            for (int col = 0; col < nColumns; col++)
            {
                for (int row = 0; row < nRows; row++)
                {
                    if (row != rowToDelete && col != colToDelete)
                    {
                        rtn[curSpot] = elementAt(col,row);
                        curSpot++;
                    }
                }
            }   

            return rtn;
        }
};



// -------------------------------------------------------------------
// ColVector: a column-based vector.

template< int height, typename T >
class ColVector : public Matrix< 1, height, T >
{
public:
        typedef Matrix<1,height,T> parentType;
    typedef ColVector< height, T > _Myt;

    inline ColVector(parentType &p) { parentType::operator=(p); }
    inline void operator=(const parentType &p) { parentType::operator=(p); }

    // accessors
    template< typename C >
    inline _Myt operator/(const C n)      const { return parentType::operator/(n); }

    template< typename C >
    inline _Myt operator+(const C n)      const { return parentType::operator+(n); }

    template< typename C >
    inline _Myt operator-(const C n)      const { return parentType::operator-(n); }

    inline _Myt operator*(int n)          const { return parentType::operator*(n);}
    inline _Myt operator*(float n)        const { return parentType::operator*(n);}
    inline _Myt operator*(double n)       const { return parentType::operator*(n);}

        inline bool operator==(const _Myt &m) const { return parentType::operator==((parentType)m);}

    // mutators
    inline void operator+=(const _Myt &m)       {        parentType::operator+=((parentType)m);}
    inline void operator-=(const _Myt &m)       {        parentType::operator-=((parentType)m);}
    inline void operator= (const _Myt &m)       {        parentType::operator=((parentType)m);}
    
    
    template <int theirCols, int theirRows>
    inline Matrix< theirCols, height, T> operator* (const Matrix< theirCols, theirRows, T> &m) const
    {
        
        // If it fails to compile here, that means that you've tried to 
        // multiply two matrices whose sizes don't match. The first
        // Matrix should have as many columns as the second Matrix's rows
        COMPILETIME_ASSERT (1==theirRows);
        
        return multMatrix< _Myt, Matrix< theirCols, theirRows, T >, T, nRows, theirCols>(*this, m);
    }
    
    Matrix <height, 1, T> getTranspose() const
        {
                Matrix<height, 1, T> rtn;
        memcpy(rtn.getData(), const_cast<_Myt*>(this)->getData(), sizeof(array));
                return rtn;
        }

    // constructors
    ColVector () {}

    ColVector (T a) 
    { 
        COMPILETIME_ASSERT (nRows == 1); array[0] = a; 
    }

    ColVector (T a, T b) 
    { 
        COMPILETIME_ASSERT (nRows == 2); 
        array[0] = a; array[1] = b;
    }

    ColVector (T a, T b, T c) 
    { 
        COMPILETIME_ASSERT (nRows == 3); array[0] = a; 
        array[0] = a; array[1] = b; array[2] = c;
    }

    ColVector (T a, T b, T c, T d) 
    { 
        COMPILETIME_ASSERT (nRows == 4); array[0] = a; 
        array[0] = a; array[1] = b; array[2] = c; array[3] = d;
    }
    
    // accessors
    T getLength () const
    { 
        return (T)sqrt(getLengthSquared()); 
    }
    
    T getLengthSquared() const
    {
        return (T)std::inner_product(&array[0], &array[nRows], &array[0], 0.);
    }

    ColVector<3, T>  crossProduct    (const ColVector<height, T> &vx2) const
    {
        const ColVector<3, T> &vx1 = *this;
        ColVector<3, T> v;
        
        v[0] = vx1[1]*vx2[2] - vx1[2]*vx2[1];
        v[1] = vx1[2]*vx2[0] - vx1[0]*vx2[2];
        v[2] = vx1[0]*vx2[1] - vx1[1]*vx2[0];
        return v;
    }

    _Myt getReflection(const _Myt & normal) const
    {
        _Myt rtn;
        rtn = *this - normal * 2. * this->dotProduct(normal);

        return rtn;
    }

    T dotProduct (const _Myt &v) const
    {
        T rtn = 0;
        for (int i = 0; i < height; i++)
            rtn += array[i] * v[i];

        return rtn;
    }

    void normalize()
    {   
        T len = getLength(); 
        if (len) {
            for (int i = 0; i < nRows; i++)
                array[i] /= len;
        }
        else
            assert(!"ColVector::normalize(): can't normalize a zero vector.");
    }
        
#if 0 // this is too dangerous to be used in practice
    ColVector( T first, ... )
    {
        va_list marker;
        
        array[0] = first;

        va_start( marker, first );     /* Initialize variable arguments. */
        for (int i = 1; i < nRows; i++)
        {
            array[i] = va_arg( marker, T);
        }
        va_end( marker );              /* Reset variable arguments.      */
    }
#endif

};




template< int height, typename T >
class Position : public Matrix< 1, height, T >
{
    public:
            typedef Matrix< 1,height,T > parentType;
        typedef Position< height, T > _Myt;

        inline Position(parentType &p) { parentType::operator=(p); }
        inline void operator=(const parentType &p) { parentType::operator=(p); }

        // constructors
        Position () {}

        Position (T a) 
        { 
            COMPILETIME_ASSERT (nRows == 1); array[0] = a; 
        }

        Position (T a, T b) 
        { 
            COMPILETIME_ASSERT (nRows == 2); 
            array[0] = a; array[1] = b;
        }

        Position (T a, T b, T c) 
        { 
            COMPILETIME_ASSERT (nRows == 3); array[0] = a; 
            array[0] = a; array[1] = b; array[2] = c;
        }

        Position (T a, T b, T c, T d) 
        { 
            COMPILETIME_ASSERT (nRows == 4); array[0] = a; 
            array[0] = a; array[1] = b; array[2] = c; array[3] = d;
        }

        _Myt linearlyInterpolate (const _Myt & p, double t) 
        {
            return ((*this)*(1-t) + (p*t));
        }

            inline bool operator==(const _Myt &m) const { return parentType::operator==((parentType)m);}
        inline void operator= (const _Myt &m)       {        parentType::operator=((parentType)m);}

        Matrix <height, 1, T> getTranspose() const
            {
                    Matrix<height, 1, T> rtn;
            memcpy(rtn.getData(), const_cast<_Myt*>(this)->getData(), sizeof(array));
                    return rtn;
            }

        template< typename C >
        inline ColVector< height, T > operator-(const C n)      const { return parentType::operator-(n); }

    // it doesn't really make sense to perform the following operations on a position.
    // what does it mean to add two places in space? nothing. interpolating makes sense,
    // but that operation is provided above.

        // accessors
        template< typename C >
        inline _Myt operator/(const C n)      const { return parentType::operator/(n); }

        template< typename C >
        inline _Myt operator+(const C &n)      const { return parentType::operator+(n); }


        inline _Myt operator*(int n)          const { return parentType::operator*(n);}
        inline _Myt operator*(float n)        const { return parentType::operator*(n);}
        inline _Myt operator*(double n)       const { return parentType::operator*(n);}

        // mutators
        inline void operator+=(const _Myt &m)       {        parentType::operator+=((parentType)m);}
        inline void operator-=(const _Myt &m)       {        parentType::operator-=((parentType)m);}
    
    
        template <int theirCols, int theirRows>
        inline Matrix< theirCols, height, T> operator* (const Matrix< theirCols, theirRows, T> &m) const
        {
        
            // If it fails to compile here, that means that you've tried to 
            // multiply two matrices whose sizes don't match. The first
            // Matrix should have as many columns as the second Matrix's rows
            COMPILETIME_ASSERT (1==theirRows);
        
            return multMatrix< _Myt, Matrix< theirCols, theirRows, T >, T, nRows, theirCols>(*this, m);
        }    
};



// -------------------------------------------------------------------
// multMatrix: matrix support function. Performs matrix
// multiplication.

template< typename A, typename B, typename T, int rows1, int cols2>
inline Matrix< cols2, rows1, T> multMatrix (A m1, B m2)
{
    Matrix< m2.nColumns, m1.nRows, T> rtn;
    int row1, col2;
    for (row1 = 0; row1 < m1.nRows; row1++)
    {
        for (col2 = 0; col2 < B::nColumns; col2++)
        {
            T result = 0;

            for (int i = 0; i < A::nColumns; i++)
                result += m1.elementAt (i, row1) * m2.elementAt (col2, i);

            rtn (col2, row1) = result;
        }
    }

    return rtn;
}


// -------------------------------------------------------------------
// printMatrix: matrix support function. Prints a matrix out to the
// debugging output window in VC++

template < typename T >
void printMatrix (const T & mtx, const char *str)
{
    MTRACE("\n---- %s -----", str);
    
    if (T::nRows == 1)
    {
        if (T::nColumns == 1)
            MTRACE("scalar");
        else
            MTRACE("%d element row vector", T::nColumns);
    }
    else
    {
        if (T::nColumns == 1)
            MTRACE("%d element col vector", T::nRows);
        else
            MTRACE("%d cols x %d rows matrix", T::nColumns, T::nRows);
    }
    
    MTRACE(" ----------\n");
    for (int row = 0; row < T::nRows; row++)
    {
        for (int col = 0; col < T::nColumns; col++)
            MTRACE ("%.3lf ", mtx.elementAt(col,row));
        
        MTRACE("\n");
    }
}

---