gls_matrix.h

Go to the documentation of this file.

/*! \file
  \brief  The GlsMatrix class.

  \par Copyright Information

  Copyright (c) 2015 by The DiSTI Corporation.<br>
  11301 Corporate Blvd; Suite 100<br>
  Orlando, Florida 32817<br>
  USA<br>
  <br>
  All rights reserved.<br>

  This Software contains proprietary trade secrets of DiSTI and may not be
reproduced, in whole or part, in any form, or by any means of electronic,
mechanical, or otherwise, without the written permission of DiSTI.  Said
permission may be derived through the purchase of applicable DiSTI product
licenses which detail the distribution rights of this content and any
Derivative Works based on this or other copyrighted DiSTI Software.

  NO WARRANTY. THE SOFTWARE IS PROVIDED "AS-IS," WITHOUT WARRANTY OF ANY KIND,
AND ANY USE OF THIS SOFTWARE PRODUCT IS AT YOUR OWN RISK. TO THE MAXIMUM EXTENT
PERMITTED BY APPLICABLE LAW, DISTI AND ITS SUPPLIERS DISCLAIM ALL WARRANTIES
AND CONDITIONS, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
IMPLIED WARRANTIES AND CONDITIONS OF MERCHANTABILITY AND/OR FITNESS FOR A
PARTICULAR PURPOSE, TITLE, AND NON-INFRINGEMENT, WITH REGARD TO THE SOFTWARE.

  LIMITATION OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW,
IN NO EVENT SHALL DISTI OR ITS SUPPLIERS BE LIABLE FOR ANY SPECIAL, INCIDENTAL,
INDIRECT, OR CONSEQUENTIAL DAMAGES WHATSOEVER (INCLUDING, WITHOUT LIMITATION,
DAMAGES FOR LOSS OF BUSINESS PROFITS, BUSINESS INTERRUPTION, LOSS OF BUSINESS
INFORMATION, OR ANY OTHER PECUNIARY LOSS) ARISING OUT OF THE USE OF OR
INABILITY TO USE THE SOFTWARE, EVEN IF DISTI HAS BEEN ADVISED OF THE POSSIBILITY
OF SUCH DAMAGES.  DISTI'S ENTIRE LIABILITY AND YOUR EXCLUSIVE REMEDY SHALL NOT
EXCEED FIVE DOLLARS (US$5.00).

  The aforementioned terms and restrictions are governed by the laws of the
State of Florida and the United States of America.

*/
#ifndef GLS_MATRIX_H
#define GLS_MATRIX_H

#include <memory>
#include <cstring>
#include <math.h>
#include <iostream>

namespace disti
{

//----------------------------------------------------------------------------
/**
* The GlsMatrix class provides support for an n x n square matrix of any 
* numeric type. Most basic matrix operations are supported.  Internally,
* the matrix terms are stored contiguously in column-major order so that the
* matrix data can be passed to OpenGL matrix operations.  However, instances
* of this class can be treated as row-major matrices since the API performs
* the conversion to column-major ordering for the user.
*/
//----------------------------------------------------------------------------
template <class Type, int DIM = 4>
class GlsMatrix
{
public:
    typedef Type CStyleMatrix[DIM][DIM];

    //------------------------------------------------------------------------
    /** 
    * Default constructor. The new matrix will be the identity matrix.
    */
    //------------------------------------------------------------------------
    GlsMatrix();

    //------------------------------------------------------------------------
    /** 
    * Constructor. This will transpose m so that the matrix is stored in
    * column-major order, but a caller can pass one in that is in row-major
    * order, which is more intuitive.
    *
    * \param m  a row-major, two-dimensional array to initialize the matrix 
    *           with.
    */
    //------------------------------------------------------------------------
    GlsMatrix(const CStyleMatrix m);

    //------------------------------------------------------------------------
    /** 
    * Constructor.
    *
    * \param m  a column-major, one-dimensional array containing all values
    *           to initialize the matrix with. 
    */
    //------------------------------------------------------------------------
    GlsMatrix(const Type* m);

    //------------------------------------------------------------------------
    /** 
    * Copy Constructor.
    *
    * \param m  matrix to copy. 
    */
    //------------------------------------------------------------------------
    GlsMatrix(const GlsMatrix<Type, DIM>& m);

    //------------------------------------------------------------------------
    /** 
    * Destructor.
    */
    //------------------------------------------------------------------------
    virtual ~GlsMatrix();

    //------------------------------------------------------------------------
    /** 
    * Dump the contents of the matrix to standard output.
    */
    //------------------------------------------------------------------------
    void Dump() const;

    //------------------------------------------------------------------------
    /** 
    * Zero out the matrix so that all terms are equal to zero.
    */
    //------------------------------------------------------------------------
    inline void Clear() { memset(_data, 0, sizeof(Type)*DIM*DIM); }

    //------------------------------------------------------------------------
    /** 
    * Provides access to the raw column-major matrix data as a one dimensional
    * array in a format suitable for Open GL matrix operations.
    */
    //------------------------------------------------------------------------
    inline Type* Data() { return _data; }
    
    //------------------------------------------------------------------------
    /** 
    * Provides access to the raw column-major matrix data as a one dimensional
    * array in a format suitable for Open GL matrix operations.
    */
    //------------------------------------------------------------------------
    inline const Type* Data() const { return _data; }

    //------------------------------------------------------------------------
    /** 
    * Set this matrix to the identity matrix. The identity matrix is 
    * I = [Aij], where Aii = 1 and Aij = 0 for i != j. Thus, for the identity 
    * matrix, the terms off the main diagonal are all zero, and the terms on 
    * the main diagonal are all one.
    */
    //------------------------------------------------------------------------
    void MakeIdentity();

    //------------------------------------------------------------------------
    /** 
    * Calculates the determinant of the matrix.
    *
    * \return the calculated value of the determinant.
    */
    //------------------------------------------------------------------------
    Type Determinant() const;

    //------------------------------------------------------------------------
    /** 
    * Calculates the inverse of this matrix. The inverse of matrix A is B if
    * AB = BA = I, where I is the identity matrix. The inverse of a matrix is
    * the notion corresponding to the reciprocal of a nonzero real number.
    *
    * \return the inverse of this matrix.
    */
    //------------------------------------------------------------------------
    GlsMatrix<Type, DIM> Inverse() const;

    //------------------------------------------------------------------------
    /** 
    * Sets this matrix equal to its inverse.
    */
    //------------------------------------------------------------------------
    void Invert();

    //------------------------------------------------------------------------
    /** 
    * Calculates the transpose of this matrix.  The transpose of a matrix
    * is obtained by interchanging its rows and columns.
    *
    * \return the transpose of this matrix.
    */
    //------------------------------------------------------------------------
    GlsMatrix<Type, DIM> Transposition() const;

    //------------------------------------------------------------------------
    /** 
    * Sets this matrix equal to its transpose.  
    */
    //------------------------------------------------------------------------
    void Transpose();

    //------------------------------------------------------------------------
    /** 
    * Determines if this is a diagonal matrix.  An n x n matrix A = [Aij] is
    * called a diagonal matrix if Aij = 0 for i != j. Thus, for a diagonal
    * matrix, the terms off the main diagonal are all zero.
    *
    * \return true if the matrix is a diagonal matrix, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsDiagonal() const;

    //------------------------------------------------------------------------
    /** 
    * Determines if this matrix is equal to the identity matrix. The 
    * identity matrix is I = [Aij], where Aii = 1 and Aij = 0 for i != j.
    * Thus, for the identity matrix, the terms off the main diagonal are all
    * zero, and the terms on the main diagonal are all one.
    *
    * \return true if the matrix is the identity matrix, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsIdentity() const;

    //------------------------------------------------------------------------
    /** 
    * Determines if this is a scalar matrix.  A scalar matrix is a diagonal
    * matrix whose diagonal terms are equal.
    *
    * \return true if the matrix is a scalar matrix, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsScalar() const;

    //------------------------------------------------------------------------
    /** 
    * Determines if this matrix is singular, or non-invertible.  A singular
    * matrix has no inverse.
    *
    * \return true if the matrix is singular, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsSingular() const;

    //------------------------------------------------------------------------
    /** 
    * Determines if this matrix is symmetric. A matrix is symmetric if it is
    * equal to its transpose, i.e. A = A'.
    *
    * \return true if the matrix is symmetric, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsSymmetric() const;

    //------------------------------------------------------------------------
    /** 
    * Determines if this matrix is skew symmetric. A matrix is skew symmetric
    * if its transpose is equal to its negative, i.e. -A = A'.
    *
    * \return true if the matrix is skew symmetric, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsSkewSymmetric() const;

    //------------------------------------------------------------------------
    /** 
    * Determines if this matrix is lower triangular. An n x n matrix A = [Aij]
    * is lower triangular if Aij = 0 for i < j.
    *
    * \return true if the matrix is lower triangular, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsLowerTriangular() const;

    //------------------------------------------------------------------------
    /** 
    * Determines if this matrix is upper triangular. An n x n matrix A = [Aij]
    * is upper triangular if Aij = 0 for i > j.
    *
    * \return true if the matrix is upper triangular, false otherwise.
    */
    //------------------------------------------------------------------------
    bool IsUpperTriangular() const;

    //------------------------------------------------------------------------
    // Operators
    //------------------------------------------------------------------------
    // Subscript operator (zero-based)
    Type& operator () (unsigned row, unsigned col);
    Type  operator () (unsigned row, unsigned col) const;

    // Equality operators
    bool operator == (const GlsMatrix<Type,DIM>& rhs) const;
    bool operator != (const GlsMatrix<Type,DIM>& rhs) const;
    
    // Assignment operators
    GlsMatrix<Type, DIM>& operator = (const GlsMatrix<Type,DIM>& m);
    GlsMatrix<Type, DIM>& operator = (const CStyleMatrix m);
    
    // Unary operators
    GlsMatrix<Type, DIM> operator + () const { return *this; }
    GlsMatrix<Type, DIM> operator - () const;

    // Combined assignment - calculation operators
    GlsMatrix<Type, DIM>& operator += (const GlsMatrix<Type,DIM>& rhs);
    GlsMatrix<Type, DIM>& operator -= (const GlsMatrix<Type,DIM>& rhs);
    GlsMatrix<Type, DIM>& operator *= (const GlsMatrix<Type,DIM>& rhs);
    GlsMatrix<Type, DIM>& operator *= (Type scalar);
    GlsMatrix<Type, DIM>& operator /= (Type scalar);

    // Mathematical operators
    GlsMatrix<Type, DIM> operator + (const GlsMatrix<Type,DIM>& rhs) const;
    GlsMatrix<Type, DIM> operator - (const GlsMatrix<Type,DIM>& rhs) const;
    GlsMatrix<Type, DIM> operator * (const GlsMatrix<Type,DIM>& rhs) const;
    GlsMatrix<Type, DIM> operator * (Type scalar) const;
    GlsMatrix<Type, DIM> operator / (Type scalar) const;

protected:
    /** A very tiny number, close to zero. Used to compare floats */
    static const double PRECISION;

    /**
    * This is the actual matrix data stored in column-major ordering in
    * a one-dimensional array of the appropriate size to store all matrix
    * terms. The data is stored in this manner so that it can be passed
    * to OpenGL matrix operations, which expect matrices to be ordered
    * this way.
    */
    Type _data[DIM*DIM];

    /** 
    * A column index array. Each element in this array "points" 
    * to a column of the matrix within the above data. By using this index,
    * we can address matrix terms using two dimensions, column and row, 
    * without incurring a multiplication. Eg. matrix[0] points to a column
    * within data, which is the address of the first term in the column.
    * We can further address into the column and access a particular row
    * by adding another dimension, eg. matrix[0][1].  The indices must
    * be set up at construction time.
    */
    Type* _matrix[DIM];

    //------------------------------------------------------------------------
    /** 
    * This should be called from all constructors, as it initializes the
    * matrix column index so that matrix elements can be addressed in two
    * dimensions, but a multiplication is not incurred when doing so.
    */
    //------------------------------------------------------------------------
    void Initialize();

    //------------------------------------------------------------------------
    /** 
    * Pivoting is used in conjunction with Gauss-Jordan reduction to find
    * the inverse of a matrix and in Gaussian Elimination to find the 
    * determinant of a matrix.  It involves the elementary row operation of
    * interchanging rows (or columns in our column-major matrix).  Only the
    * row passed in and rows below it are considered.  The row passed in
    * will be swapped with another row below it by selecting the desired
    * pivot element.  The pivot is the largest term (in magnitude) in 
    * the column equal to row for any of the considered rows.  This will 
    * always put the largest (in magnitude) term from the column on the 
    * diagonal of the matrix.
    *
    * \param row is the current row to pivot about. Rows above this are not
    *        considered during pivoting, as these rows should already be in
    *        their proper form.
    */
    //------------------------------------------------------------------------
    int Pivot(unsigned row);

    //------------------------------------------------------------------------
    /** 
    * Decides if the two values are close together determined by the
    * constant PRECISION. The formula is 
    *               lv - PRECISION < rv < lv + PRECISION
    *
    * \param lv  left value
    * \param rv  right value
    */
    //------------------------------------------------------------------------
    bool closeValues(Type lv, Type rv) const;
};



//============================================================================
//
//                         CLASS IMPLEMENTATION
//
//============================================================================

//----------------------------------------------------------------------------
template<class Type, int DIM>
const double GlsMatrix<Type, DIM>::PRECISION = 1e-7;

//----------------------------------------------------------------------------
template<class Type, int DIM>
GlsMatrix<Type, DIM>::GlsMatrix()
{
    Initialize();
    MakeIdentity();
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
GlsMatrix<Type, DIM>::GlsMatrix(const CStyleMatrix m)
{
    Initialize();
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            _matrix[col][row] = m[row][col];
        }
    }
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
GlsMatrix<Type, DIM>::GlsMatrix(const Type* m)
{
    Initialize();
    memcpy(_data, m, sizeof(Type)*DIM*DIM); 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
GlsMatrix<Type, DIM>::GlsMatrix(const GlsMatrix<Type,DIM>& m) 
{ 
    Initialize();
    memcpy(_data, m._data, sizeof(Type)*DIM*DIM); 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
GlsMatrix<Type, DIM>::~GlsMatrix()
{
}
//----------------------------------------------------------------------------
template<>
inline
GlsMatrix<double, 4> 
GlsMatrix<double, 4>::operator * (const GlsMatrix<double, 4>& rhs) const
{
    GlsMatrix<double, 4> result;

    // Same as regular, just unroll the loop
    result._matrix[0][0] = _matrix[0][0] * rhs._matrix[0][0] + _matrix[1][0] * rhs._matrix[0][1] + _matrix[2][0] * rhs._matrix[0][2] + _matrix[3][0] * rhs._matrix[0][3];
    result._matrix[1][0] = _matrix[0][0] * rhs._matrix[1][0] + _matrix[1][0] * rhs._matrix[1][1] + _matrix[2][0] * rhs._matrix[1][2] + _matrix[3][0] * rhs._matrix[1][3];
    result._matrix[2][0] = _matrix[0][0] * rhs._matrix[2][0] + _matrix[1][0] * rhs._matrix[2][1] + _matrix[2][0] * rhs._matrix[2][2] + _matrix[3][0] * rhs._matrix[2][3];
    result._matrix[3][0] = _matrix[0][0] * rhs._matrix[3][0] + _matrix[1][0] * rhs._matrix[3][1] + _matrix[2][0] * rhs._matrix[3][2] + _matrix[3][0] * rhs._matrix[3][3];
    result._matrix[0][1] = _matrix[0][1] * rhs._matrix[0][0] + _matrix[1][1] * rhs._matrix[0][1] + _matrix[2][1] * rhs._matrix[0][2] + _matrix[3][1] * rhs._matrix[0][3];
    result._matrix[1][1] = _matrix[0][1] * rhs._matrix[1][0] + _matrix[1][1] * rhs._matrix[1][1] + _matrix[2][1] * rhs._matrix[1][2] + _matrix[3][1] * rhs._matrix[1][3];
    result._matrix[2][1] = _matrix[0][1] * rhs._matrix[2][0] + _matrix[1][1] * rhs._matrix[2][1] + _matrix[2][1] * rhs._matrix[2][2] + _matrix[3][1] * rhs._matrix[2][3];
    result._matrix[3][1] = _matrix[0][1] * rhs._matrix[3][0] + _matrix[1][1] * rhs._matrix[3][1] + _matrix[2][1] * rhs._matrix[3][2] + _matrix[3][1] * rhs._matrix[3][3];
    result._matrix[0][2] = _matrix[0][2] * rhs._matrix[0][0] + _matrix[1][2] * rhs._matrix[0][1] + _matrix[2][2] * rhs._matrix[0][2] + _matrix[3][2] * rhs._matrix[0][3];
    result._matrix[1][2] = _matrix[0][2] * rhs._matrix[1][0] + _matrix[1][2] * rhs._matrix[1][1] + _matrix[2][2] * rhs._matrix[1][2] + _matrix[3][2] * rhs._matrix[1][3];
    result._matrix[2][2] = _matrix[0][2] * rhs._matrix[2][0] + _matrix[1][2] * rhs._matrix[2][1] + _matrix[2][2] * rhs._matrix[2][2] + _matrix[3][2] * rhs._matrix[2][3];
    result._matrix[3][2] = _matrix[0][2] * rhs._matrix[3][0] + _matrix[1][2] * rhs._matrix[3][1] + _matrix[2][2] * rhs._matrix[3][2] + _matrix[3][2] * rhs._matrix[3][3];
    result._matrix[0][3] = _matrix[0][3] * rhs._matrix[0][0] + _matrix[1][3] * rhs._matrix[0][1] + _matrix[2][3] * rhs._matrix[0][2] + _matrix[3][3] * rhs._matrix[0][3];
    result._matrix[1][3] = _matrix[0][3] * rhs._matrix[1][0] + _matrix[1][3] * rhs._matrix[1][1] + _matrix[2][3] * rhs._matrix[1][2] + _matrix[3][3] * rhs._matrix[1][3];
    result._matrix[2][3] = _matrix[0][3] * rhs._matrix[2][0] + _matrix[1][3] * rhs._matrix[2][1] + _matrix[2][3] * rhs._matrix[2][2] + _matrix[3][3] * rhs._matrix[2][3];
    result._matrix[3][3] = _matrix[0][3] * rhs._matrix[3][0] + _matrix[1][3] * rhs._matrix[3][1] + _matrix[2][3] * rhs._matrix[3][2] + _matrix[3][3] * rhs._matrix[3][3];

    return result;
}
//----------------------------------------------------------------------------
template<>
inline
GlsMatrix<float, 4> 
GlsMatrix<float, 4>::operator * (const GlsMatrix<float, 4>& rhs) const
{
    GlsMatrix<float, 4> result;

    // Same as regular, just unroll the loop
    result._matrix[0][0] = _matrix[0][0] * rhs._matrix[0][0] + _matrix[1][0] * rhs._matrix[0][1] + _matrix[2][0] * rhs._matrix[0][2] + _matrix[3][0] * rhs._matrix[0][3];
    result._matrix[1][0] = _matrix[0][0] * rhs._matrix[1][0] + _matrix[1][0] * rhs._matrix[1][1] + _matrix[2][0] * rhs._matrix[1][2] + _matrix[3][0] * rhs._matrix[1][3];
    result._matrix[2][0] = _matrix[0][0] * rhs._matrix[2][0] + _matrix[1][0] * rhs._matrix[2][1] + _matrix[2][0] * rhs._matrix[2][2] + _matrix[3][0] * rhs._matrix[2][3];
    result._matrix[3][0] = _matrix[0][0] * rhs._matrix[3][0] + _matrix[1][0] * rhs._matrix[3][1] + _matrix[2][0] * rhs._matrix[3][2] + _matrix[3][0] * rhs._matrix[3][3];
    result._matrix[0][1] = _matrix[0][1] * rhs._matrix[0][0] + _matrix[1][1] * rhs._matrix[0][1] + _matrix[2][1] * rhs._matrix[0][2] + _matrix[3][1] * rhs._matrix[0][3];
    result._matrix[1][1] = _matrix[0][1] * rhs._matrix[1][0] + _matrix[1][1] * rhs._matrix[1][1] + _matrix[2][1] * rhs._matrix[1][2] + _matrix[3][1] * rhs._matrix[1][3];
    result._matrix[2][1] = _matrix[0][1] * rhs._matrix[2][0] + _matrix[1][1] * rhs._matrix[2][1] + _matrix[2][1] * rhs._matrix[2][2] + _matrix[3][1] * rhs._matrix[2][3];
    result._matrix[3][1] = _matrix[0][1] * rhs._matrix[3][0] + _matrix[1][1] * rhs._matrix[3][1] + _matrix[2][1] * rhs._matrix[3][2] + _matrix[3][1] * rhs._matrix[3][3];
    result._matrix[0][2] = _matrix[0][2] * rhs._matrix[0][0] + _matrix[1][2] * rhs._matrix[0][1] + _matrix[2][2] * rhs._matrix[0][2] + _matrix[3][2] * rhs._matrix[0][3];
    result._matrix[1][2] = _matrix[0][2] * rhs._matrix[1][0] + _matrix[1][2] * rhs._matrix[1][1] + _matrix[2][2] * rhs._matrix[1][2] + _matrix[3][2] * rhs._matrix[1][3];
    result._matrix[2][2] = _matrix[0][2] * rhs._matrix[2][0] + _matrix[1][2] * rhs._matrix[2][1] + _matrix[2][2] * rhs._matrix[2][2] + _matrix[3][2] * rhs._matrix[2][3];
    result._matrix[3][2] = _matrix[0][2] * rhs._matrix[3][0] + _matrix[1][2] * rhs._matrix[3][1] + _matrix[2][2] * rhs._matrix[3][2] + _matrix[3][2] * rhs._matrix[3][3];
    result._matrix[0][3] = _matrix[0][3] * rhs._matrix[0][0] + _matrix[1][3] * rhs._matrix[0][1] + _matrix[2][3] * rhs._matrix[0][2] + _matrix[3][3] * rhs._matrix[0][3];
    result._matrix[1][3] = _matrix[0][3] * rhs._matrix[1][0] + _matrix[1][3] * rhs._matrix[1][1] + _matrix[2][3] * rhs._matrix[1][2] + _matrix[3][3] * rhs._matrix[1][3];
    result._matrix[2][3] = _matrix[0][3] * rhs._matrix[2][0] + _matrix[1][3] * rhs._matrix[2][1] + _matrix[2][3] * rhs._matrix[2][2] + _matrix[3][3] * rhs._matrix[2][3];
    result._matrix[3][3] = _matrix[0][3] * rhs._matrix[3][0] + _matrix[1][3] * rhs._matrix[3][1] + _matrix[2][3] * rhs._matrix[3][2] + _matrix[3][3] * rhs._matrix[3][3];

    return result;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM> 
GlsMatrix<Type, DIM>::operator * (const GlsMatrix<Type, DIM>& rhs)  const
{ 
    GlsMatrix<Type,DIM> result;
    memset(result._data, 0, sizeof(Type) * DIM * DIM);
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            for (int i = 0; i < DIM; ++i)
            {
                result._matrix[col][row] += _matrix[i][row] * rhs._matrix[col][i];
            }
        }
    }
    return result; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM> GlsMatrix<Type, DIM>::operator * (Type scalar)  const
{ 
    GlsMatrix<Type, DIM> result;
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            result._matrix[col][row] = _matrix[col][row] * scalar;
        }
    }
    return result; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
void GlsMatrix<Type, DIM>::Dump() const
{
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            std::cout.width(11);
            std::cout << _matrix[col][row] << ' ';
        }
        std::cout << std::endl;
    }
    std::cout << std::endl;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
void GlsMatrix<Type, DIM>::MakeIdentity()
{
    Clear();
    for (int i = 0; i < DIM; ++i)
    {
        _matrix[i][i] = Type(1);
    }
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
Type GlsMatrix<Type, DIM>::Determinant() const
{
    // In this algorithm we use gaussian elimination with pivoting to reduce
    // the matrix to an upper triangular form.  The determinant of an
    // upper triangular matrix is the product of the terms on the diagonal.
    // To perform the elimination, we will use two types of elementary row 
    // operations on the matrix. 
    // 1) Interchanging rows of the matrix.
    // 2) Add c times row i of the matrix to row j where i != j.
    //
    // The first operation of interchanging rows prevents numerical
    // instability, and the second allows us to zero out terms below
    // the diagonal by choosing c appropriately.

    Type pivotFactor;
    Type det = Type(1);

    // Make a temporary copy of the matrix so the original is not destroyed 
    // during gaussian elimination and pivoting
    GlsMatrix<Type,DIM> temp(*this);
    
    // Loop through each row in the matrix
    for (int k = 0; k < DIM; ++k)
    {
        // Perform pivoting
        int index = temp.Pivot(k);

        // If pivot returns a -1, then a zero exists on the diagonal
        // so the determinant will be zero.
        if (index == -1)
            return 0;
        // If pivot returns a number other than zero, rows have been
        // interchanged, so we need to change the sign of the determinant
        // due to the theorem that states "If matrix B results from A by
        // interchanging two rows (columns) of A, then det(B) = - det(A).
        if (index != 0)
            det = -det;

        // The determinant will be the product of the diagonal terms. The
        // row that the [k][k] term is in already has the proper form 
        // for an upper triangular matrix, so go ahead and use it in the det.
        det *= temp._matrix[k][k];

        // Since we are using the diagonal element from row k, column k, we
        // need all other terms below this element, in column k, to be
        // zero. We do this by an elementary operation (2) above to each
        // row below this diagonal element. 
        for (int i = k+1; i < DIM; ++i)
        {
            // Choose the pivot factor that would zero out the [i][k]term
            // when multiplied by the kth row and subtracted from the 
            // ith row.
            pivotFactor = temp._matrix[i][k] / temp._matrix[k][k];

            // Perform the operation on each term in the row to the right
            // of the pivot. The terms to the left are assumed to be zero
            // and we never reference them again so no need to waste time
            // processing them.
            for (int j = k+1; j < DIM; ++j)
            {
                temp._matrix[i][j] -= pivotFactor * temp._matrix[k][j];
            }
        }
    }
    return det;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
GlsMatrix<Type, DIM> GlsMatrix<Type, DIM>::Inverse() const
{
    // In this algorithm we use Gauss-Jordan reduction with pivoting to 
    // transform the original matrix to the identity matrix, all the while
    // performing the same operations on what started out as the identity
    // matrix.  What started out as the identity matrix will become the
    // inverse of the original matrix. This is because A*Inv(A) = I and
    // performing the same operations on either side of the equation to
    // reduce A to the identity matrix, the equation becomes I*Inv(A) = Inv(A).
    // To perform the elimination, we will use two types of elementary row 
    // operations on the matrix. 
    // 1) Interchanging rows of the matrix.
    // 2) Multiply row i of the matrix by c, where c != 0.
    // 2) Add c times row i of the matrix to row j where i != j.
    //
    // The first operation of interchanging rows prevents numerical
    // instability. The second allows us to put 1's on the diagonal, by
    // dividing the row by the value of the term on the diagonal.
    // and the third to zero out terms that are not on the diagonal by
    // choosing c appropriately.

    Type a1_inv, a2;
    
    // Make a temporary copy of the matrix so the original is not destroyed 
    // during Gauss-Jordan reduction and pivoting
    GlsMatrix<Type,DIM> temp(*this);

    // This starts out as the identity matrix. Every operation we perform 
    // on temp, we will also do to this matrix to maintain the equality in
    // the equation we are starting out with: A*Inv(A) = I
    GlsMatrix<Type,DIM> inverse;

    // Loop through each row in the matrix
    for (int k = 0; k < DIM; ++k)
    {
        // Perform pivoting
        int index = temp.Pivot(k);

        // If pivot returns a -1, then the matrix is singular or
        // non-invertible. TBD: WHAT SHOULD WE DO HERE?
        if (index == -1)
            return inverse;

        // If pivoting resulted in the swapping of rows, then
        // we need to do the same thing to the inverse matrix
        if (index != 0)
        {
            // Swap rows
            Type rowptr[DIM];
            size_t byteCount = sizeof(Type) * DIM;
            memcpy(rowptr, inverse._matrix[k], byteCount);
            memcpy(inverse._matrix[k], inverse._matrix[index], byteCount);
            memcpy(inverse._matrix[index], rowptr, byteCount);
        }

        // Divide the current row by the term on the diagonal so we
        // get a 1 on the diagonal. Remember, do the same operation
        // to the inverse matrix.
        a1_inv = ((Type)1.0)/temp._matrix[k][k];
        for (int j = 0; j < DIM; ++j)
        {
            temp._matrix[k][j]    *= a1_inv;
            inverse._matrix[k][j] *= a1_inv;
        }

        // Now, loop through each row, skipping the current
        // one (k).
        // Choose the pivot factor (a2) that would zero out the [i][k]term
        // when multiplied by the kth row and subtracted from the 
        // ith row.  Since the kth row has a 1 in the diagonal term,
        // the factor should be the [i][k] term itself.  This loop results
        // in zeros in the kth column, except for the diagonal term [k][k].
        for (int i = 0; i < DIM; ++i)
        {
            if (i != k)
            {
                a2 = temp._matrix[i][k];
                for (int j = 0; j < DIM; ++j)
                {
                    temp._matrix[i][j]    -= a2 * temp._matrix[k][j];
                    inverse._matrix[i][j] -= a2 * inverse._matrix[k][j];
                }
            }
        }
    }
    return inverse;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
void GlsMatrix<Type, DIM>::Invert()
{
    *this = Inverse();
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM> GlsMatrix<Type, DIM>::Transposition() const
{ 
    // Interchange rows with columns
    GlsMatrix<Type, DIM> result;
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            result._matrix[col][row] = _matrix[row][col];
        }
    }
    return result; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
void GlsMatrix<Type, DIM>::Transpose()
{ 
    GlsMatrix<Type, DIM> result;
    result = Transposition();
    *this = result;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsDiagonal() const
{ 
    // Look for the first term off the diagonal (where row != column) that
    // is not equal to zero. If we find one, the matrix is not a diagonal.
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            if (col != row && !closeValues(_matrix[col][row],Type(0)))
                return false;
        }
    }
    return true;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsIdentity() const
{ 
    // First do a quick check to see if it is an un-touched identity matrix
    // This allows for zero tolerences, but should be much faster than
    // doing all the tolerance checking every time.
    static const GlsMatrix referenceIdentity; // Defaults to identity.
    if (0 == memcmp(_data, referenceIdentity._data, sizeof(Type) * DIM * DIM))
        return true;


    // If the matrix is scalar, it is diagonal and all terms on the diagonal
    // are equal. Make sure they are equal to 1 to be the identity matrix.
    if (IsScalar() && closeValues(_matrix[0][0],Type(1)))
        return true;
    else
        return false;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsScalar() const
{ 
    // If the matrix is scalar, it is diagonal and all terms on the diagonal
    // are equal.
    if (!IsDiagonal())
        return false;
    
    Type val = _matrix[0][0];
    for (int i = 0; i < DIM; ++i)
    {
        if (!closeValues(_matrix[i][i],val))
            return false;
    }
    return true;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsSingular() const
{ 
    return Determinant() == Type(0);
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsSymmetric() const
{ 
    return Transposition() == *this;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsSkewSymmetric() const
{ 
    return Transposition() == -(*this);
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsLowerTriangular() const
{ 
    // An n x n matrix A = [Aij] is lower triangular if Aij = 0 for i < j.
    for (int row = 0; row < DIM-1; ++row)
    {
        for (int col = row+1; col < DIM; ++col)
        {
            if (!closeValues(_matrix[col][row],Type(0)))
                return false;
        }
    }
    return true;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::IsUpperTriangular() const
{ 
    // An n x n matrix A = [Aij] is upper triangular if Aij = 0 for i > j.
    for (int row = 1; row < DIM; ++row)
    {
        for (int col = 0; col < row; ++col)
        {
            if (!closeValues(_matrix[col][row],Type(0)))
                return false;
        }
    }
    return true;
}
//----------------------------------------------------------------------------
template<class Type, int DIM> 
inline 
Type& GlsMatrix<Type, DIM>::operator () (unsigned row, unsigned col)
{
    // transpose the row and column since the matrix is in column
    // major order but we want users to be able to use row-major ordering.
    return _matrix[col][row];
}
//----------------------------------------------------------------------------
template<class Type, int DIM> 
inline 
Type GlsMatrix<Type, DIM>::operator () (unsigned row, unsigned col) const
{
    // transpose the row and column since the matrix is in column
    // major order but we want users to be able to use row-major ordering.
    return _matrix[col][row];
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::operator == (const GlsMatrix<Type, DIM>& rhs) const
{
    static const int SIZE = DIM*DIM;
    for (int i = 0; i < SIZE; ++i)
    {
        if (!closeValues(_data[i],rhs._data[i]))
            return false;
    }
    return true;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::operator != (const GlsMatrix<Type, DIM>& rhs) const
{ 
    return !(*this == rhs);
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline GlsMatrix<Type, DIM>& 
GlsMatrix<Type, DIM>::operator = (const GlsMatrix<Type, DIM>& m) 
{ 
    if (this == &m) return *this;
    
    memcpy(_data, m._data, sizeof(Type)*DIM*DIM); 
    
    return *this; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline GlsMatrix<Type, DIM>& 
GlsMatrix<Type, DIM>::operator = (const CStyleMatrix m) 
{ 
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            _matrix[col][row] = m[row][col];
        }
    }
    
    return *this; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM> GlsMatrix<Type, DIM>::operator - () const
{
    GlsMatrix<Type,DIM> temp;
    
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            temp._matrix[col][row] = -_matrix[col][row];
        }
    }
        
    return temp;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM>& 
GlsMatrix<Type, DIM>::operator += (const GlsMatrix<Type, DIM>& rhs) 
{ 
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            _matrix[col][row] += rhs._matrix[col][row];
        }
    }
    return *this; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM>& 
GlsMatrix<Type, DIM>::operator -= (const GlsMatrix<Type, DIM>& rhs) 
{ 
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            _matrix[col][row] -= rhs._matrix[col][row];
        }
    }
    return *this; 
}
//----------------------------------------------------------------------------
template<>
inline
GlsMatrix<double, 4>&
GlsMatrix<double, 4>::operator *= (const GlsMatrix<double,4>& rhs)
{
    return *this = *this * rhs; // faster because it calls the specialized operator*
}
//----------------------------------------------------------------------------
template<>
inline
GlsMatrix<float, 4>&
GlsMatrix<float, 4>::operator *= (const GlsMatrix<float,4>& rhs)
{
    return *this = *this * rhs; // faster because it calls the specialized operator*
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM>& 
GlsMatrix<Type, DIM>::operator *= (const GlsMatrix<Type, DIM>& rhs) 
{ 
    GlsMatrix<Type,DIM> result;
    memset(result._data, 0, sizeof(Type) * DIM * DIM);
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            for (int i = 0; i < DIM; ++i)
            {
                result._matrix[col][row] += _matrix[i][row] * rhs._matrix[col][i];
            }
        }
    }
    *this = result; 
    return *this; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM>& 
GlsMatrix<Type, DIM>::operator *= (Type scalar) 
{ 
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            _matrix[col][row] *= scalar;
        }
    }
    return *this; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM>& 
GlsMatrix<Type, DIM>::operator /= (Type scalar) 
{ 
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            _matrix[col][row] /= scalar;
        }
    }
    return *this; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM> 
GlsMatrix<Type, DIM>::operator + (const GlsMatrix<Type, DIM>& rhs) const
{ 
    GlsMatrix<Type,DIM> result;
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            result._matrix[col][row] = _matrix[col][row] + rhs._matrix[col][row];
        }
    }
    return result; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM> 
GlsMatrix<Type, DIM>::operator - (const GlsMatrix<Type, DIM>& rhs)  const
{ 
    GlsMatrix<Type,DIM> result;
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            result._matrix[col][row] = _matrix[col][row] - rhs._matrix[col][row];
        }
    }
    return result; 
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
GlsMatrix<Type, DIM> GlsMatrix<Type, DIM>::operator / (Type scalar)  const
{ 
    GlsMatrix<Type, DIM> result;
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            result._matrix[col][row] = _matrix[col][row] / scalar;
        }
    }
    return result; 
}
//----------------------------------------------------------------------------
template <class Type, int DIM> 
inline
std::ostream& operator << (std::ostream& ostrm, const GlsMatrix<Type, DIM>& m)
{
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            ostrm.width(11);
            ostrm << m(row,col) << ' ';
        }
    }
    return ostrm;
}
//----------------------------------------------------------------------------
template <class Type, int DIM> 
inline
std::istream& operator >> (std::istream& istrm, GlsMatrix<Type, DIM>& m)
{
    Type x;
    for (int row = 0; row < DIM; ++row)
    {
        for (int col = 0; col < DIM; ++col)
        {
            istrm >> x;
            m(row,col) = x;
        }
    }
    return istrm;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
void GlsMatrix<Type, DIM>::Initialize()
{
    // Set each element in the matrix index to point to a column of the
    // _matrix data.
    for (int i = 0, j = 0; i < DIM; ++i,j+=DIM)
    {
        _matrix[i] = &(_data[j]);
    }
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
int GlsMatrix<Type, DIM>::Pivot(unsigned row)
{
    int    pivotRow;
    int    k    = int(row);
    double amax = -1;
    double temp;
    
    // Go through each element in the column below the diagonal element
    // [row][row] to find the largest term (in magnitude). Save which row
    // has the largest term in the column.
    for (unsigned i = row; i < unsigned(DIM); ++i)
    {
        if ((temp = fabs(_matrix[i][row])) > amax && temp != 0.0)
        {
            amax = temp;
            k = i;
        }
    }

    if (_matrix[k][row] == Type(0))
    {
        // If the largest term is zero, then return a -1 so the caller knows
        // we cannot pivot.
        pivotRow = -1;
    }
    else if (k != int(row))
    {
        // If k is not equal to the passed in row, it means another row
        // has the largest term in it, so lets interchange rows.
        // Return the row that was interchanged to let the caller know.
        Type rowptr[DIM];
        size_t byteCount = sizeof(Type) * DIM;
        memcpy(rowptr, _matrix[k], byteCount);
        memcpy(_matrix[k], _matrix[row], byteCount);
        memcpy(_matrix[row], rowptr, byteCount);
        pivotRow = k;
    }
    else
    {
        // If k is equal to the passed in row, it means current row
        // has the largest term in it, so lets not interchange any rows.
        // return 0 to let the caller know.
        pivotRow = 0;
    }

    return pivotRow;
}
//----------------------------------------------------------------------------
template<class Type, int DIM>
inline
bool GlsMatrix<Type, DIM>::closeValues(Type lv, Type rv) const
{
    return (lv - PRECISION) <= rv && rv <= (lv + PRECISION);
}


} // namespace disti

#endif