gls_matrix_affine.h

Go to the documentation of this file.

/*! \file
  \brief  The GlsMatrixAffine 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_MATRIXAFFINE_H
#define GLS_MATRIXAFFINE_H

#include "gls_matrix.h"
#include "vertex.h"
#include "gls_quaternion.h"

namespace disti
{

//----------------------------------------------------------------------------
/**
* The GlsMatrixAffine class provides support for a 4x4 affine transformation
* matrix of any numeric type. It provides ability to perform rotations,
* scaling, and translations.  Results are cummulative, i.e. every 
* rotate, scale, and translate call transforms the terms that already exist 
* in the matrix.  Call MakeIdentity() to start with an identity matrix, or
* Clear() to clear the matrix with all zeros.
*/
//----------------------------------------------------------------------------
template <class Type>
class GlsMatrixAffine : public GlsMatrix<Type, 4>
{
public:
    using GlsMatrix<Type,4>::_matrix;
    typedef GlsMatrix<Type, 4> _BaseClass;

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

    //------------------------------------------------------------------------
    /** 
    * Constructor.
    *
    * \param m  a row-major, two-dimensional array used to initialize the 
    *           matrix.
    */
    //------------------------------------------------------------------------
    GlsMatrixAffine(const Type m[4][4]);

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

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

    //------------------------------------------------------------------------
    /** 
    * Destructor.
    */
    //------------------------------------------------------------------------
    ~GlsMatrixAffine();

    /** \return True if the supplied matrix is "very close" to this one.
      */
    bool IsVeryClose(const GlsMatrix<Type,4> & b) const
    {
        static const unsigned int SIZE = 4*4;
        static const double PRECISION = 0.00001;

        for (unsigned int i = 0; i < SIZE; ++i)
        {
            if (fabs(this->_data[i] - b.Data()[i]) > PRECISION)
                return false;
        }
        return true;
    }

    //------------------------------------------------------------------------
    /** 
    * Perform a rotation transformation around a major axis around the origin, 
    * and combine this transformation with the existing matrix data.
    *
    * \param angle             rotation angle in degrees
    * \param axis              major axis of rotation: X_AXIS, Y_AXIS, Z_AXIS
    */
    //------------------------------------------------------------------------
    void Rotate(Type angle, RotationAxis axis);

    //------------------------------------------------------------------------
    /** 
    * Perform a rotation transformation around a major axis using a specified
    * center of rotation, and combine this transformation with the existing
    * matrix data.
    *
    * \param angle             rotation angle in degrees
    * \param centerOfRotation  point to rotate around
    * \param axis              major axis of rotation: X_AXIS, Y_AXIS, Z_AXIS
    */
    //------------------------------------------------------------------------
    void Rotate(Type angle, const Vector& centerOfRotation, RotationAxis axis);

    //------------------------------------------------------------------------
    /** 
    * Perform a rotation transformation around an arbitrary axis using a 
    * specified center of rotation, and combine this transformation with the 
    * existing matrix data.
    *
    * \param angle             rotation angle in degrees
    * \param centerOfRotation  point to rotate around
    * \param axis              arbitray axis of rotation
    */
    //------------------------------------------------------------------------
    void Rotate(Type angle, const Vector& centerOfRotation, const Vector& axis);

    //------------------------------------------------------------------------
    /** 
    * Perform a scaling transformation using the origin as the anchor point,
    * and combine this transformation with the existing matrix data.
    *
    * \param x scaling factor in the x direction. 1 = 100% (no scaling change)
    * \param y scaling factor in the y direction. 1 = 100% (no scaling change)
    * \param z scaling factor in the z direction. 1 = 100% (no scaling change)
    */
    //------------------------------------------------------------------------
    void Scale(Type x, Type y, Type z);

    //------------------------------------------------------------------------
    /** 
    * Perform a scaling transformation using an arbitrary anchor point,
    * and combine this transformation with the existing matrix data.
    *
    * \param x scaling factor in the x direction. 1 = 100% (no scaling change)
    * \param y scaling factor in the y direction. 1 = 100% (no scaling change)
    * \param z scaling factor in the z direction. 1 = 100% (no scaling change)
    * \param anchor The anchor point for the scaling
    */
    //------------------------------------------------------------------------
    void Scale(Type x, Type y, Type z, const Vector& anchor);

    //------------------------------------------------------------------------
    /** 
    * Perform a translation, and combine this transformation with the 
    * existing matrix data.
    *
    * \param x translation in the x direction
    * \param y translation in the y direction
    * \param z translation in the z direction
    */
    //------------------------------------------------------------------------
    inline void Translate(Type x, Type y, Type z)
    {
        // The translation matrix is:
        //     |1 0 0 x|
        // T = |0 1 0 y|
        //     |0 0 1 z|
        //     |0 0 0 1|
        // We multiply it on the left: *this = T * *this;

        _matrix[0][0] += x * _matrix[0][3];
        _matrix[0][1] += y * _matrix[0][3];
        _matrix[0][2] += z * _matrix[0][3];
        //_matrix[0][3] = _matrix[0][3];
        _matrix[1][0] += x * _matrix[1][3];
        _matrix[1][1] += y * _matrix[1][3];
        _matrix[1][2] += z * _matrix[1][3];
        //_matrix[1][3] = _matrix[1][3];
        _matrix[2][0] += x * _matrix[2][3];
        _matrix[2][1] += y * _matrix[2][3];
        _matrix[2][2] += z * _matrix[2][3];
        //_matrix[2][3] = _matrix[2][3];
        _matrix[3][0] += x * _matrix[3][3];
        _matrix[3][1] += y * _matrix[3][3];
        _matrix[3][2] += z * _matrix[3][3];
        //_matrix[3][3] = _matrix[3][3];
    }

    //------------------------------------------------------------------------
    /** 
    * Perform a translation, and combine this transformation with the 
    * existing matrix data.
    *
    * \param v containing the translate values
    */
    //------------------------------------------------------------------------
    inline void Translate(const Vector& v) { Translate((Type)v.x, (Type)v.y, (Type)v.z); }

    // LynxOS does not support 'using'
    using _BaseClass::operator*;

    //------------------------------------------------------------------------
    /** 
    * Return a transformed vertex using the specified vertex and the data in 
    * the matrix,  e.g. w = M * v
    */
    //------------------------------------------------------------------------
    inline Vector operator * (const Vector& v) const
    {
        const GlsMatrix<Type,4>& M = *this;
        return Vector(
            float( (M(0,0) * v.x) + (M(0,1) * v.y) + (M(0,2) * v.z) + M(0,3) ),
            float( (M(1,0) * v.x) + (M(1,1) * v.y) + (M(1,2) * v.z) + M(1,3) ),
            float( (M(2,0) * v.x) + (M(2,1) * v.y) + (M(2,2) * v.z) + M(2,3) )
            );
    }
    inline Vertex operator * (const Vertex& v) const
    {
        const GlsMatrix<Type,4>& M = *this;
        return Vertex(
            float( (M(0,0) * v.x) + (M(0,1) * v.y) + (M(0,2) * v.z) + M(0,3) ),
            float( (M(1,0) * v.x) + (M(1,1) * v.y) + (M(1,2) * v.z) + M(1,3) ),
            float( (M(2,0) * v.x) + (M(2,1) * v.y) + (M(2,2) * v.z) + M(2,3) ),
            v.color.R(),
            v.color.G(),
            v.color.B(),
            v.color.A());
    }

    //------------------------------------------------------------------------
    /** 
    * Transform the specified vertex using the data in the matrix, where the 4th vector component is assumed to be a 1 (position)
    * i.e. v = M * v
    */
    //------------------------------------------------------------------------
    inline void Transform(Vector& v) const
    {
        // Simple version is:
        // v = *this * v;

        // Faster version is:
        float x = float( (_matrix[0][0] * v.x) + (_matrix[1][0] * v.y) + (_matrix[2][0] * v.z) + _matrix[3][0] );
        float y = float( (_matrix[0][1] * v.x) + (_matrix[1][1] * v.y) + (_matrix[2][1] * v.z) + _matrix[3][1] );
        float z = float( (_matrix[0][2] * v.x) + (_matrix[1][2] * v.y) + (_matrix[2][2] * v.z) + _matrix[3][2] );   
        v.x = x;
        v.y = y;
        v.z = z;
    }
};

/** decompose the scale factors from a GlsMatrix
    * \param[in] matrix the matrix to decompose
    * \return scale a vector that contains the x, y, and z scale factors
    */
template <class Type>
Vector DecomposeScale( const GlsMatrix<Type,4>& m )
{
    const Vector xAxis = Vector( float( m(0,0) ), float( m(1,0) ), float( m(2,0) ) );
    const Vector yAxis = Vector( float( m(0,1) ), float( m(1,1) ), float( m(2,1) ) );
    const Vector zAxis = Vector( float( m(0,2) ), float( m(1,2) ), float( m(2,2) ) );
    return Vector( xAxis.Magnitude(), yAxis.Magnitude(), zAxis.Magnitude() );
}

/** decompose the rotation matrix from a GlsMatrix by dividing out the scale factor. If the x,y, or z scale is determined 
    * to be zero, the scale factors not removed from the matrix.
    * \param[in] matrix the matrix to decompose
    * \param[in] scale a vector that contains the x, y, and z scale factors
    * \return the rotation matrix with the scale factors removed
    */
template <class Type>
GlsMatrix<Type,4> DecomposeRotationMatrix( const GlsMatrix<Type,4>& matrix, const Vector& scale )
{
    GlsMatrix<Type,4> m( matrix );

    // set the translation to 0
    m(0,3) = Type(); 
    m(1,3) = Type(); 
    m(2,3) = Type(); 

    // only decompose rotation if all scales are non-zero
    if( 0 != scale.x && 0 != scale.y && 0 != scale.z )
    {
        m(0,0) /= scale.x;
        m(1,0) /= scale.x;
        m(2,0) /= scale.x;
        m(0,1) /= scale.y;
        m(1,1) /= scale.y;
        m(2,1) /= scale.y;
        m(0,2) /= scale.z;
        m(1,2) /= scale.z;
        m(2,2) /= scale.z;
    }

    return m;
}

/** decompose a GlsMatrix into its scaling, translation, and rotation counterparts. If the x,y, or z scale is determined 
    * to be zero, the rotation may not be accurate
    * \param[in] matrix the matrix to decompose
    * \param[out] scale a vector that contains the x, y, and z scale factors
    * \param[out] location a vector that contains the x, y, and z translation
    * \param[out] rotation a quaternion that contains the rotation
    */
template <class Type>
void DecomposeMatrix( const GlsMatrix<Type,4>& matrix, Vector& scale, Vector& location, GlsQuaternion<Type>& rotation )
{
    scale = DecomposeScale( matrix );

    // decompose location
    location = Vector( float( matrix(0,3) ), float(  matrix(1,3) ), float(  matrix(2,3) ) );

    rotation = GlsQuaternion<Type>( DecomposeRotationMatrix( matrix, scale ) );
}


typedef GlsMatrixAffine<float> GlsMatrixAffineF;
typedef GlsMatrixAffine<double> GlsMatrixAffineD;

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

//----------------------------------------------------------------------------
template<class Type>
GlsMatrixAffine<Type>::GlsMatrixAffine()
{
}
//----------------------------------------------------------------------------
template<class Type>
GlsMatrixAffine<Type>::GlsMatrixAffine(const Type m[4][4]) :
    GlsMatrix<Type,4>(m)
{
}
//----------------------------------------------------------------------------
template<class Type>
GlsMatrixAffine<Type>::GlsMatrixAffine(const Type* m) : 
    GlsMatrix<Type,4>(m)
{
}
//----------------------------------------------------------------------------
template<class Type>
GlsMatrixAffine<Type>::GlsMatrixAffine(const GlsMatrix<Type,4>& m) :
    GlsMatrix<Type,4>(m) 
{
}
//----------------------------------------------------------------------------
template<class Type>
GlsMatrixAffine<Type>::~GlsMatrixAffine()
{
}

//----------------------------------------------------------------------------
template<class Type>
void GlsMatrixAffine<Type>::Rotate(Type angle, RotationAxis axis)
{
    // Find the sine and cosine of the angle of rotation to be used
    // in the rotation transform matrix.
    const double a   (double(angle) * DEG_TO_RAD);
    const Type   sina( (Type) sin(a) );
    const Type   cosa( (Type) cos(a) );

    Type temp;
    
    switch (axis)
    {
        case X_AXIS:
        {
            // The rotation matrix for rotation about the x axis is
            //     |1   0       0    0|
            // R = |0 cos(a) -sin(a) 0|
            //     |0 sin(a)  cos(a) 0|
            //     |0   0       0    1|    
            
            // Multiply the current matrix by the rotation transformation
            // *this = R * *this;

            temp = _matrix[0][1];
            _matrix[0][1] = temp * cosa - _matrix[0][2] * sina;
            _matrix[0][2] = temp * sina + _matrix[0][2] * cosa;
            temp = _matrix[1][1];
            _matrix[1][1] = temp * cosa - _matrix[1][2] * sina;
            _matrix[1][2] = temp * sina + _matrix[1][2] * cosa;
            temp = _matrix[2][1];
            _matrix[2][1] = temp * cosa - _matrix[2][2] * sina;
            _matrix[2][2] = temp * sina + _matrix[2][2] * cosa;
            temp = _matrix[3][1];
            _matrix[3][1] = temp * cosa - _matrix[3][2] * sina;
            _matrix[3][2] = temp * sina + _matrix[3][2] * cosa;

            break;
        }
        case Y_AXIS:
        {
            // The rotation matrix for rotation about the y axis is
            //     | cos(a) 0 sin(a) 0|
            // R = |   0    1   0    0|
            //     |-sin(a) 0 cos(a) 0|
            //     |   0    0   0    1|    

            // Multiply the current matrix by the rotation transformation
            // *this = R * *this;

            temp = _matrix[0][0];
            _matrix[0][0] = temp * cosa + _matrix[0][2] * sina;
            _matrix[0][2] = _matrix[0][2] * cosa - temp * sina;
            temp = _matrix[1][0];
            _matrix[1][0] = temp * cosa + _matrix[1][2] * sina;
            _matrix[1][2] = _matrix[1][2] * cosa - temp * sina;
            temp = _matrix[2][0];
            _matrix[2][0] = temp * cosa + _matrix[2][2] * sina;
            _matrix[2][2] = _matrix[2][2] * cosa - temp * sina;
            temp = _matrix[3][0];
            _matrix[3][0] = temp * cosa + _matrix[3][2] * sina;
            _matrix[3][2] = _matrix[3][2] * cosa - temp * sina;

            break;
        }
        case Z_AXIS:
        {
            // The rotation matrix for rotation about the z axis is
            //     |cos(a)  sin(a) 0  0|
            // R = |-sin(a) cos(a) 0  0|
            //     |  0       0    1  0|
            //     |  0       0    0  1|    

            // Multiply the current matrix by the rotation transformation
            // *this = R * *this;

            temp = _matrix[0][0];
            _matrix[0][0] = temp * cosa - _matrix[0][1] * sina;
            _matrix[0][1] = temp * sina + _matrix[0][1] * cosa;
            temp = _matrix[1][0];
            _matrix[1][0] = temp * cosa - _matrix[1][1] * sina;
            _matrix[1][1] = temp * sina + _matrix[1][1] * cosa;
            temp = _matrix[2][0];
            _matrix[2][0] = temp * cosa - _matrix[2][1] * sina;
            _matrix[2][1] = temp * sina + _matrix[2][1] * cosa;
            temp = _matrix[3][0];
            _matrix[3][0] = temp * cosa - _matrix[3][1] * sina;
            _matrix[3][1] = temp * sina + _matrix[3][1] * cosa;

            break;
        }
    }
}
//----------------------------------------------------------------------------
template<class Type>
void GlsMatrixAffine<Type>::Rotate(Type          angle, 
                                   const Vector& centerOfRotation, 
                                   RotationAxis  axis)
{
    // First, we must translate the center of rotation to the origin, 
    // perform the rotation and then translate back to the center of 
    // rotation.
    
    // Find the sine and cosine of the angle of rotation to be used
    // in the rotation transform matrix.
    const double a   (double(angle) * DEG_TO_RAD);
    const Type   sina( (Type) sin(a) );
    const Type   cosa( (Type) cos(a) );

    Type temp;

    switch (axis)
    {
        case X_AXIS:
        {
            // The rotation matrix for rotation about the x axis is
            //     |1   0       0    0|
            // R = |0 cos(a) -sin(a) 0|
            //     |0 sin(a)  cos(a) 0|
            //     |0   0       0    1|    
            //
            // The total transformation T is:
            //     |1   0    0         x-x        |
            // T = |0  R11  R12  (-R11*y-R12*z)+y |
            //     |0  R21  R22  (-R21*y-R22*z)+z |
            //     |0   0    0          1         |
            const Type T11(cosa), T12(-sina), T21(sina), T22(cosa),
                 // T03( centerOfRotation.x-centerOfRotation.x ),
                    T13( centerOfRotation.y-(cosa*centerOfRotation.y-sina*centerOfRotation.z) ),
                    T23( centerOfRotation.z-(sina*centerOfRotation.y+cosa*centerOfRotation.z) );

            // Multiply by the transformation matrix
            // *this = T * *this;
            temp = _matrix[0][1];
            _matrix[0][1] = temp * T11 + _matrix[0][2] * T12 + _matrix[0][3] * T13;
            _matrix[0][2] = temp * T21 + _matrix[0][2] * T22 + _matrix[0][3] * T23;
            temp = _matrix[1][1];
            _matrix[1][1] = temp * T11 + _matrix[1][2] * T12 + _matrix[1][3] * T13;
            _matrix[1][2] = temp * T21 + _matrix[1][2] * T22 + _matrix[1][3] * T23;
            temp = _matrix[2][1];
            _matrix[2][1] = temp * T11 + _matrix[2][2] * T12 + _matrix[2][3] * T13;
            _matrix[2][2] = temp * T21 + _matrix[2][2] * T22 + _matrix[2][3] * T23;
            temp = _matrix[3][1];
            _matrix[3][1] = temp * T11 + _matrix[3][2] * T12 + _matrix[3][3] * T13;
            _matrix[3][2] = temp * T21 + _matrix[3][2] * T22 + _matrix[3][3] * T23;

            break;
        }
        case Y_AXIS:
        {
            // The rotation matrix for rotation about the y axis is
            //     | cos(a) 0 sin(a) 0|
            // R = |   0    1   0    0|
            //     |-sin(a) 0 cos(a) 0|
            //     |   0    0   0    1|    
            //
            // The total transformation T is:
            //     |R00  0  R02 (-R00*x-R02*z)+x |
            // T = | 0   1   0        y-y        |
            //     |R20  0  R22 (-R20*x-R22*z)+z |
            //     | 0   0   0         1         |
            const Type T00(cosa), T02(sina), T20(-sina), T22(cosa),
                    T03( centerOfRotation.x-(sina*centerOfRotation.z+cosa*centerOfRotation.x) ),
                 // T13( centerOfRotation.y-centerOfRotation.y ),
                    T23( centerOfRotation.z-(cosa*centerOfRotation.z-sina*centerOfRotation.x) );

            // Multiply by the transformation matrix
            // *this = T * *this;
            temp = _matrix[0][0];
            _matrix[0][0] = temp * T00 + _matrix[0][2] * T02 + _matrix[0][3] * T03;
            _matrix[0][2] = temp * T20 + _matrix[0][2] * T22 + _matrix[0][3] * T23;
            temp = _matrix[1][0];
            _matrix[1][0] = temp * T00 + _matrix[1][2] * T02 + _matrix[1][3] * T03;
            _matrix[1][2] = temp * T20 + _matrix[1][2] * T22 + _matrix[1][3] * T23;
            temp = _matrix[2][0];
            _matrix[2][0] = temp * T00 + _matrix[2][2] * T02 + _matrix[2][3] * T03;
            _matrix[2][2] = temp * T20 + _matrix[2][2] * T22 + _matrix[2][3] * T23;
            temp = _matrix[3][0];
            _matrix[3][0] = temp * T00 + _matrix[3][2] * T02 + _matrix[3][3] * T03;
            _matrix[3][2] = temp * T20 + _matrix[3][2] * T22 + _matrix[3][3] * T23;

            break;
        }
        case Z_AXIS:
        {
            // The rotation matrix for rotation about the z axis is
            //     |cos(a) -sin(a) 0  0|
            // R = |sin(a)  cos(a) 0  0|
            //     |  0       0    1  0|
            //     |  0       0    0  1|    
            //
            // The total transformation T is:
            //     |R00 R01  0 (-R00*x-R01*y)+x |
            // T = |R10 R11  0 (-R10*x-R11*y)+y |
            //     | 0   0   1       z-z        |
            //     | 0   0   0        1         |
            const Type T00(cosa), T01(-sina), T10(sina), T11(cosa),
                    T03( centerOfRotation.x-(cosa*centerOfRotation.x-sina*centerOfRotation.y) ),
                    T13( centerOfRotation.y-(sina*centerOfRotation.x+cosa*centerOfRotation.y) );
                 // T23( centerOfRotation.z-centerOfRotation.z );

            // Multiply by the transformation matrix
            // *this = T * *this;
            temp = _matrix[0][0];
            _matrix[0][0] = temp * T00 + _matrix[0][1] * T01 + _matrix[0][3] * T03;
            _matrix[0][1] = temp * T10 + _matrix[0][1] * T11 + _matrix[0][3] * T13;
            temp = _matrix[1][0];
            _matrix[1][0] = temp * T00 + _matrix[1][1] * T01 + _matrix[1][3] * T03;
            _matrix[1][1] = temp * T10 + _matrix[1][1] * T11 + _matrix[1][3] * T13;
            temp = _matrix[2][0];
            _matrix[2][0] = temp * T00 + _matrix[2][1] * T01 + _matrix[2][3] * T03;
            _matrix[2][1] = temp * T10 + _matrix[2][1] * T11 + _matrix[2][3] * T13;
            temp = _matrix[3][0];
            _matrix[3][0] = temp * T00 + _matrix[3][1] * T01 + _matrix[3][3] * T03;
            _matrix[3][1] = temp * T10 + _matrix[3][1] * T11 + _matrix[3][3] * T13;

            break;
        }
    }
}
//----------------------------------------------------------------------------
template<class Type>
void GlsMatrixAffine<Type>::Rotate(Type          angle, 
                                   const Vector& centerOfRotation, 
                                   const Vector& axis)
{
    // Normalize the arbitrary axis into a unit vector to be used in the
    // calculations for the terms of the rotation matrix.  Only perform
    // the rotation if the length of the axis vector is not zero.
    Vector u(axis);
    u.Normalize();
        
    if (u.MagnitudeSquared() != 0)
    {
        // The following describes the theory used to determine the rotation 
        // matrix for a rotation around an arbitrary axis:
        //
        // Definitions:
        //
        // The "dual matrix" of a vector u(x,y,z) is S, where
        //     | 0 -z  y |
        // S = | z  0 -x |
        //     |-y  x  0 |
        //
        // The "outer product" of a vector u with itself (where uT is
        // the transpose of u) is:
        //       |x|          |xx xy xz|
        // uuT = |y||x y z| = |xy yy yz|
        //       |z|          |xz yz zz|
        //
        // Suppose we have a unit direction vector, u, then we can derive
        // a rotation by a given angle about this axis:
        //
        // R(a,u) = uuT + (I - uuT) * cos(a) + S * sin(a)
        //
        // where I is the identity matrix. This actually produces a 3x3 
        // matrix which we just need to add the 4th column and row to so 
        // that we can use it for affine transformations.
        
        // Calculate some constants from uuT
        const Type xx(Type(u.x * u.x));
        const Type yy(Type(u.y * u.y));
        const Type zz(Type(u.z * u.z));
        const Type xy(Type(u.x * u.y));
        const Type xz(Type(u.x * u.z));
        const Type yz(Type(u.y * u.z));

        // Calculate the sine and cosine of the rotation angle for use
        // in the calculation of the rotation matrix.
        const double a   (double(angle) * DEG_TO_RAD);
        const Type   sina( (Type) sin(a) );
        const Type   cosa( (Type) cos(a) );
        
        // Now calculate M, which is the 3x3 version of the rotation
        // matrix using the formula for rotation about an arbitrary axis.
        // GlsMatrix<Type,3> M = UUT + ((I - UUT) * cosa) + (S * sina);
        
        // Assign M to the first 3 columns and rows of the final rotation
        // matrix R.
        GlsMatrix<Type,4> R;
        
        R(0,0) = xx + (1-xx)*cosa;
        R(1,0) = xy - xy*cosa     + u.z*sina;
        R(2,0) = xz - xz*cosa     - u.y*sina;
        
        R(0,1) = xy - xy*cosa     - u.z*sina;
        R(1,1) = yy + (1-yy)*cosa;
        R(2,1) = yz - yz*cosa     + u.x*sina;
        
        R(0,2) = xz - xz*cosa     + u.y*sina;
        R(1,2) = yz - yz*cosa     - u.x*sina;
        R(2,2) = zz + (1-zz)*cosa;

        // Multiply the current matrix by the translation, then the rotation,
        // and then translate back using the inverse.
        // *this = TInv * R * T * *this;
        this->Translate(-centerOfRotation.x, -centerOfRotation.y, -centerOfRotation.z);
        *this = R * *this;
        this->Translate(centerOfRotation.x, centerOfRotation.y, centerOfRotation.z);
    }
}
//----------------------------------------------------------------------------
template<class Type>
void GlsMatrixAffine<Type>::Scale(Type x, Type y, Type z)
{
   // The scaling matrix, where x, y, and z are on the diagonal.
   // x 0 0 0
   // 0 y 0 0
   // 0 0 z 0
   // 0 0 0 1
   // As can be seen most of the matrix is zero
   // So optimized

    _matrix[0][0] = _matrix[0][0] * x;
    _matrix[0][1] = _matrix[0][1] * y;
    _matrix[0][2] = _matrix[0][2] * z;
    //_matrix[0][3] = _matrix[0][3];

    _matrix[1][0] = _matrix[1][0] * x;
    _matrix[1][1] = _matrix[1][1] * y;
    _matrix[1][2] = _matrix[1][2] * z;
    //_matrix[1][3] = _matrix[1][3];

    _matrix[2][0] = _matrix[2][0] * x;
    _matrix[2][1] = _matrix[2][1] * y;
    _matrix[2][2] = _matrix[2][2] * z;
    //_matrix[2][3] = _matrix[2][3];

    _matrix[3][0] = _matrix[3][0] * x;
    _matrix[3][1] = _matrix[3][1] * y;
    _matrix[3][2] = _matrix[3][2] * z;
    //_matrix[3][3] = _matrix[3][3];



   // Row 3 is multiplied by 1, so no need...
}
//----------------------------------------------------------------------------
template<class Type>
void GlsMatrixAffine<Type>::Scale(Type x, Type y, Type z, const Vector& anchor)
{
    // First, we must translate the anchor point to the origin, 
    // perform the scaling and then translate back to the anchor
    // point.  This is the transformation matrix we are multiplying by:
    // (Ax, Ay, Az are the components of anchor)
    //     | x  0  0  Ax-x*Ax |
    // T = | 0  y  0  Ay-y*Ay |
    //     | 0  0  z  Az-z*Az |
    //     | 0  0  0     1    |
    const Type T00 = x;
    const Type T11 = y;
    const Type T22 = z;
    const Type T03 = anchor.x*(1-x);
    const Type T13 = anchor.y*(1-y);
    const Type T23 = anchor.z*(1-z);

    // Multiply the current matrix by the transformation
    // *this = T * *this;

    _matrix[0][0] = _matrix[0][0] * T00 + _matrix[0][3] * T03;
    _matrix[0][1] = _matrix[0][1] * T11 + _matrix[0][3] * T13;
    _matrix[0][2] = _matrix[0][2] * T22 + _matrix[0][3] * T23;
    //_matrix[0][3] = _matrix[0][3];

    _matrix[1][0] = _matrix[1][0] * T00 + _matrix[1][3] * T03;
    _matrix[1][1] = _matrix[1][1] * T11 + _matrix[1][3] * T13;
    _matrix[1][2] = _matrix[1][2] * T22 + _matrix[1][3] * T23;
    //_matrix[1][3] = _matrix[1][3];

    _matrix[2][0] = _matrix[2][0] * T00 + _matrix[2][3] * T03;
    _matrix[2][1] = _matrix[2][1] * T11 + _matrix[2][3] * T13;
    _matrix[2][2] = _matrix[2][2] * T22 + _matrix[2][3] * T23;
    //_matrix[2][3] = _matrix[2][3];

    _matrix[3][0] = _matrix[3][0] * T00 + _matrix[3][3] * T03;
    _matrix[3][1] = _matrix[3][1] * T11 + _matrix[3][3] * T13;
    _matrix[3][2] = _matrix[3][2] * T22 + _matrix[3][3] * T23;
    //_matrix[3][3] = _matrix[3][3];
}

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


#ifdef MATRIX_TYPE_FLOAT
    typedef float GlsMatrixElementType;
    typedef GlsMatrixAffineF GlsMatrixType;
    #define GLS_glLoadMatrix_TYPE glLoadMatrixf
    #define GLS_glMultMatrix_TYPE glMultMatrixf
#else
    typedef double GlsMatrixElementType;
    typedef GlsMatrixAffineD GlsMatrixType;
    #define GLS_glLoadMatrix_TYPE glLoadMatrixd
    #define GLS_glMultMatrix_TYPE glMultMatrixd
#endif


} // namespace disti

#endif