#include "Math.h"

double        MATH::g_dblTolerance = 0.0000001;

template <class T, unsigned int Dimensions>
const void *  MATH::DN::Box<T, Dimensions>::m_ptrWrapValues = NULL;

template <class T, unsigned int Dimensions>
const void *  MATH::DN::Box<T, Dimensions>::m_ptrWrapDistances = NULL;

template <class T, unsigned int Dimensions>
unsigned int  MATH::DN::Box<T, Dimensions>::m_uintWrapMask = 0x1;

MATH::D2::Box MATH::D2::BoxW::m_stuWrap(-180.0f, 180.0f, -90.0f, 90.0f);
float         MATH::D2::BoxW::m_fltWrapDistances[2] = {360.0f, 180.0f};

char g_chrReturnValues[4][4][4] =
{
    {
        {0, 0, 0, 5},
        {0, 0, 0, 0},
        {0, 0, 0, 0},
        {5, 0, 0, 6},
    },
    {
        {0, 0, 0, 0},
        {0, 4, 4, 3},
        {0, 4, 4, 3},
        {0, 3, 3, 2},
    },
    {
        {0, 0, 0, 0},
        {0, 4, 4, 3},
        {0, 4, 4, 3},
        {0, 3, 3, 2},
    },
    {
        {5, 0, 0, 6},
        {0, 3, 3, 2},
        {0, 3, 3, 2},
        {6, 2, 2, 1},
    },
};

/////////////////////////////////////////////////////////////
//
// 2 Dimensions
//
/////////////////////////////////////////////////////////////

int
MATH::D2::Box::
Intersect(const MATH::D2::Point & p_stuStart,
          const MATH::D2::Point & p_stuEnd,
          MATH::D2::Point (& p_stuResults)[2]) const
{
    bool blnPointsFlipped;
    float fltPoints[2];
    float fltResults[2];
    float fltParameterIncEnd;
    float fltParameterIncStart;
    float fltParameterEnd = -1.0f;
    float fltParameterStart = -1.0f;

    for (char i=0; i<2; i++)
    {
        if ( p_stuStart[i] <= p_stuEnd[i] )
        {
            fltPoints[0] = p_stuStart[i];
            fltPoints[1] = p_stuEnd[i];
            blnPointsFlipped = false;
        }
        else
        {
            fltPoints[0] = p_stuEnd[i];
            fltPoints[1] = p_stuStart[i];
            blnPointsFlipped = true;
        }

        if ( fltPoints[0] >= m_fltValues[2 * i] && fltPoints[0] <= m_fltValues[(2 * i) + 1] )
        {
            if ( fltPoints[1] < m_fltValues[2 * i] )
                fltResults[0] = m_fltValues[2 * i];
            else if ( fltPoints[1] > m_fltValues[(2 * i) + 1] )
                fltResults[0] = m_fltValues[(2 * i) + 1];
            else
                fltResults[0] = fltPoints[1];

            fltParameterIncStart = 0.0f;
            fltParameterIncEnd = ( fltPoints[0] != fltPoints[1] )
                ? ((float)(fltPoints[0] - fltResults[0])) / ((float)(fltPoints[0] - fltPoints[1]))
                : 1.0f;
        }
        else if ( fltPoints[1] >= m_fltValues[2 * i] && fltPoints[1] <= m_fltValues[(2 * i) + 1] )
        {
            if ( fltPoints[0] < m_fltValues[2 * i] )
                fltResults[0] = m_fltValues[2 * i];
            else if ( fltPoints[0] > m_fltValues[(2 * i) + 1] )
                fltResults[0] = m_fltValues[(2 * i) + 1];
            else
                fltResults[0] = fltPoints[0];

            fltParameterIncStart = ((float)(fltResults[0] - fltPoints[0])) / ((float)(fltPoints[1] - fltPoints[0]));
            fltParameterIncEnd = 1.0f;
        }
        else if ( fltPoints[0] < m_fltValues[2 * i] && fltPoints[1] > m_fltValues[(2 * i) + 1] )
        {
            fltResults[0] = m_fltValues[2 * i];
            fltResults[1] = m_fltValues[(2 * i) + 1];
            fltParameterIncStart = ((float)(fltResults[0] - fltPoints[1])) / ((float)(fltPoints[0] - fltPoints[1]));
            fltParameterIncEnd = ((float)(fltPoints[0] - fltResults[1])) / ((float)(fltPoints[1] - fltPoints[0]));
        }
        else
            return 0;

        if ( blnPointsFlipped )
        {
            float fltTemp = 1.0f - fltParameterIncStart;
            fltParameterIncStart = 1.0f - fltParameterIncEnd;
            fltParameterIncEnd = fltTemp;
        }

        if ( i )
        {
            if ( fltParameterStart < fltParameterIncStart )
                fltParameterStart = fltParameterIncStart;
            if ( fltParameterEnd > fltParameterIncEnd )
                fltParameterEnd = fltParameterIncEnd;
            if ( fltParameterStart > fltParameterEnd )
                return 0;
        }
        else
        {
            fltParameterStart = fltParameterIncStart;
            fltParameterEnd = fltParameterIncEnd;
        }
    }

    p_stuResults[0] = p_stuStart + ((p_stuEnd - p_stuStart) * fltParameterStart);
    p_stuResults[1] = p_stuStart + ((p_stuEnd - p_stuStart) * fltParameterEnd);

    if ( fltParameterStart < fltParameterEnd )
    {
        if ( MATH::Zero(fltParameterStart) )
            return ( MATH::Equal(fltParameterEnd, 1.0f) ) ? 1 : 4;
        else
            return ( MATH::Equal(fltParameterEnd, 1.0f) ) ? 5 : 2;
    }
    else if ( MATH::Zero(fltParameterStart) )
        return 4;
    else if ( MATH::Equal(fltParameterStart, 1.0f) )
        return 5;
    else
        return 3;
}

/////////////////////////////////////////////////////////////
// BoxW
/////////////////////////////////////////////////////////////

void
MATH::D2::BoxW::
Add(const MATH::D2::Point & p_stuPoint)
{
    //
    // X dimension (wraps)
    //
    float fltX = MATH::D2::BoxW::WrapValue(p_stuPoint.m_fltX);
    if ( m_fltValues[0] > m_fltValues[1] )
    {
        if ( fltX > m_fltValues[1] &&
             fltX < m_fltValues[0] )
        {
            if ( (m_fltValues[0] - fltX) < (fltX - m_fltValues[1]) )
                m_fltValues[0] = fltX;
            else
                m_fltValues[1] = fltX;
        }
    }
    else if ( fltX < m_fltValues[0] )
    {
        if ( (m_fltValues[0] - fltX) < ((MATH::D2::BoxW::m_stuWrap[1] - m_fltValues[1]) + (fltX - MATH::D2::BoxW::m_stuWrap[0])) )
            m_fltValues[0] = fltX;
        else
            m_fltValues[1] = fltX;
    }
    else if ( fltX > m_fltValues[1] )
    {
        if ( (fltX - m_fltValues[1]) <= ((m_fltValues[0] - MATH::D2::BoxW::m_stuWrap[0]) + (MATH::D2::BoxW::m_stuWrap[1] - fltX)) )
            m_fltValues[1] = fltX;
        else
            m_fltValues[0] = fltX;
    }

    //
    // Y dimension (doesn't wrap)
    //
    float fltY = MATH::D2::BoxW::ClampValue(p_stuPoint.m_fltY);
    if ( fltY < m_fltValues[2] )
        m_fltValues[2] = fltY;
    else if ( fltY > m_fltValues[3] )
        m_fltValues[3] = fltY;
}

//
// Boxes should both be Adjust()'d before calling this
//
void
MATH::D2::BoxW::
Add(const MATH::D2::BoxW & p_stuBox)
{
    //
    // X dimension
    //
    if ( m_fltValues[0] <= m_fltValues[1] )
    {
        if ( p_stuBox[0] <= p_stuBox[1] )
        {
            if ( m_fltValues[0] >= p_stuBox[1] )
            {
                float fltDifferenceWrap = (p_stuBox[0] - MATH::D2::BoxW::m_stuWrap[0]) + (MATH::D2::BoxW::m_stuWrap[0] - m_fltValues[1]);
                float fltDifferenceNoWrap = m_fltValues[0] - p_stuBox[1];
                if ( fltDifferenceNoWrap <= fltDifferenceWrap )
                    m_fltValues[0] = p_stuBox[0];
                else
                    m_fltValues[1] = p_stuBox[1];
            }
            else if ( m_fltValues[0] > p_stuBox[0] )
                m_fltValues[0] = p_stuBox[0];
        }
        else
        {
            if ( m_fltValues[0] >= p_stuBox[0] ||
                 m_fltValues[1] <= p_stuBox[1] )
            {
                m_fltValues[0] = p_stuBox[0];
                m_fltValues[1] = p_stuBox[1];
            }
            else if ( m_fltValues[0] <= p_stuBox[1] )
            {
                if ( m_fltValues[1] >= p_stuBox[0] )
                {
                    m_fltValues[0] = MATH::D2::BoxW::m_stuWrap[0];
                    m_fltValues[1] = MATH::D2::BoxW::m_stuWrap[1];
                }
                else
                    m_fltValues[0] = p_stuBox[0];
            }
            else if ( m_fltValues[1] < p_stuBox[0] )
            {
                float fltDifferenceMin = m_fltValues[0] - p_stuBox[1];
                float fltDifferenceMax = p_stuBox[0] - m_fltValues[1];
                if ( fltDifferenceMin <= fltDifferenceMax )
                    m_fltValues[0] = p_stuBox[0];
                else
                    m_fltValues[1] = p_stuBox[1];
            }
            else
                m_fltValues[1] = p_stuBox[0];
        }
    }
    else
    {
        if ( p_stuBox[0] <= p_stuBox[1] )
        {
            if ( m_fltValues[0] > p_stuBox[0] &&
                 m_fltValues[1] < p_stuBox[1] )
            {
                if ( m_fltValues[1] >= p_stuBox[0] )
                {
                    if ( m_fltValues[0] <= p_stuBox[1] )
                    {
                        m_fltValues[0] = MATH::D2::BoxW::m_stuWrap[0];
                        m_fltValues[1] = MATH::D2::BoxW::m_stuWrap[1];
                    }
                    else
                        m_fltValues[1] = p_stuBox[1];
                }
                else if ( m_fltValues[0] > p_stuBox[1] )
                {
                    float fltDifferenceMin = p_stuBox[0] - m_fltValues[1];
                    float fltDifferenceMax = m_fltValues[0] - p_stuBox[1];
                    if ( fltDifferenceMin < fltDifferenceMax )
                        m_fltValues[1] = p_stuBox[1];
                    else
                        m_fltValues[0] = p_stuBox[0];
                }
                else
                    m_fltValues[0] = p_stuBox[0];
            }
        }
        else
        {
            if ( m_fltValues[0] > p_stuBox[0] )
                m_fltValues[0] = p_stuBox[0];
            if ( m_fltValues[1] < p_stuBox[1] )
                m_fltValues[1] = p_stuBox[1];
        }
    }

    //
    // Y dimension
    //
    if ( m_fltValues[2] > p_stuBox[2] )
        m_fltValues[2] = p_stuBox[2];
    if ( m_fltValues[3] < p_stuBox[3] )
        m_fltValues[3] = p_stuBox[3];
}

void
MATH::D2::BoxW::
Adjust(void)
{
    m_fltValues[0] = MATH::D2::BoxW::WrapValue(m_fltValues[0]);
    m_fltValues[1] = MATH::D2::BoxW::WrapValue(m_fltValues[1]);
    m_fltValues[2] = MATH::D2::BoxW::ClampValue(m_fltValues[2]);
    m_fltValues[3] = MATH::D2::BoxW::ClampValue(m_fltValues[3]);
}

bool
MATH::D2::BoxW::
Intersect(const MATH::D2::Point & p_stuPoint) const
{
    //
    // X dimension
    //
    float fltX = MATH::D2::BoxW::WrapValue(p_stuPoint.m_fltX);
    if ( m_fltValues[0] <= m_fltValues[1] )
    {
        if ( fltX < m_fltValues[0] || fltX > m_fltValues[1] )
            return false;
    }
    else if ( fltX > m_fltValues[0] && fltX < m_fltValues[1] )
        return false;

    //
    // Y dimension
    //
    float fltY = MATH::D2::BoxW::ClampValue(p_stuPoint.m_fltY);
    return ( fltY >= m_fltValues[2] && fltY <= m_fltValues[3] );
}

//
// Returns:
//
// 0 - No intersection
// 1 - Intersection, but no box is within the other.
// 2 - Same as 1, but the intersection is wrapped.
// 3 - p_ptrBox is inside this box;  this box is not inside p_ptrBox
// 4 - this box is inside p_ptrBox;  p_ptrBox is not inside this box
// 5 - both boxes are the same
//
// Boxes should both be Adjust()'d before calling this
//
int
MATH::D2::BoxW::
Intersect(const MATH::D2::BoxW & p_stuBox,
          MATH::D2::BoxW * p_ptrResults) const
{
    bool blnWrapped = false;
    bool blnOneInsideTwo = true;
    bool blnTwoInsideOne = true;

    //
    // X dimension
    //
    if ( m_fltValues[0] < m_fltValues[1] )
    {
        if ( p_stuBox[0] < p_stuBox[1] )
        {
            if ( !((m_fltValues[0] >= p_stuBox[0] && m_fltValues[0] <= p_stuBox[1]) ||
                (m_fltValues[1] >= p_stuBox[0] && m_fltValues[1] <= p_stuBox[1]) ||
                (m_fltValues[0] < p_stuBox[0] && m_fltValues[1] > p_stuBox[1])) )
            {
                return 0;
            }
            else if ( m_fltValues[0] < p_stuBox[0] || m_fltValues[1] > p_stuBox[1] )
                blnOneInsideTwo = false;
            else if ( p_stuBox[0] < m_fltValues[0] || p_stuBox[1] > m_fltValues[1] )
                blnTwoInsideOne = false;
        }
        else
        {
            blnTwoInsideOne = false;
            if ( !((m_fltValues[0] <= p_stuBox[1]) || (m_fltValues[1] >= p_stuBox[0])) )
                return 0;
            else if ( !((m_fltValues[0] >= p_stuBox[0]) || (m_fltValues[1] < p_stuBox[1])) )
                blnOneInsideTwo = false;

            if ( p_stuBox[1] >= m_fltValues[0] && p_stuBox[0] <= m_fltValues[1] )
                blnWrapped = true;
        }
    }
    else
    {
        if ( p_stuBox[0] < p_stuBox[1] )
        {
            blnOneInsideTwo = false;
            if ( !((p_stuBox[0] <= m_fltValues[1]) || (p_stuBox[1] >= m_fltValues[0])) )
                return 0;
            else if ( !((p_stuBox[0] >= m_fltValues[0]) || (p_stuBox[1] < m_fltValues[1])) )
                blnTwoInsideOne = false;

            if ( m_fltValues[1] >= p_stuBox[0] && m_fltValues[0] <= p_stuBox[1] )
                blnWrapped = true;
        }
        else
        {
            if ( m_fltValues[0] < p_stuBox[0] || m_fltValues[1] > p_stuBox[1] )
                blnOneInsideTwo = false;
            if ( p_stuBox[0] < m_fltValues[0] || p_stuBox[1] > m_fltValues[1] )
                blnTwoInsideOne = false;

            if ( m_fltValues[0] <= p_stuBox[1] || p_stuBox[0] <= m_fltValues[1] )
                blnWrapped = true;
        }
    }

    //
    // Y dimension
    //
    if ( m_fltValues[3] < p_stuBox[2] || m_fltValues[2] > p_stuBox[3] )
        return 0;
    if ( m_fltValues[2] < p_stuBox[2] || m_fltValues[3] > p_stuBox[3] )
        blnOneInsideTwo = false;
    if ( p_stuBox[2] < m_fltValues[2] || p_stuBox[3] > m_fltValues[3] )
        blnTwoInsideOne = false;


    //
    // Result
    //
    if ( blnOneInsideTwo && blnTwoInsideOne )
        return 5;
    else if ( blnOneInsideTwo )
        return 4;
    else if ( blnTwoInsideOne )
        return 3;
    else
        return ( blnWrapped == false ) ? 1 : 2;
}

MATH::D2::BoxW
MATH::D2::BoxW::
Sector(const unsigned char p_uchrSector) const
{
    MATH::D2::BoxW stuReturn;

    //
    // X axis
    //
    float fltX = ( m_fltValues[0] >= m_fltValues[1] )
        ? MATH::D2::BoxW::WrapValue(m_fltValues[0] + (((MATH::D2::BoxW::m_stuWrap[1] - m_fltValues[0]) + (m_fltValues[1] - MATH::D2::BoxW::m_stuWrap[0])) * 0.5f))
        : (m_fltValues[0] + m_fltValues[1]) * 0.5f;
    if ( p_uchrSector & 0x1 )
    {
        stuReturn[0] = fltX;
        stuReturn[1] = m_fltValues[1];
    }
    else
    {
        stuReturn[0] = m_fltValues[0];
        stuReturn[1] = fltX;
    }

    //
    // Y axis
    //
    if ( p_uchrSector & 0x2 )
    {
        stuReturn[2] = (m_fltValues[2] + m_fltValues[3]) * 0.5f;
        stuReturn[3] = m_fltValues[3];
    }
    else
    {
        stuReturn[2] = m_fltValues[2];
        stuReturn[3] = (m_fltValues[2] + m_fltValues[3]) * 0.5f;
    }

    return stuReturn;
}

/////////////////////////////////////////////////////////////
//
// 3 Dimensions
//
/////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////
// Line
/////////////////////////////////////////////////////////////

bool
MATH::D3::Line::
Intersect(const MATH::D3::Point * p_ptrPoint,
          MATH::D3::Result * p_ptrResult) const
{
    float fltValues[2];
    if ( m_stuDirection.m_fltX != 0.0f )
    {
        p_ptrResult->m_fltTime = (p_ptrPoint->m_fltX - m_stuPoint.m_fltX) / m_stuDirection.m_fltX;
        fltValues[0] = (m_stuPoint.m_fltY + (p_ptrResult->m_fltTime * m_stuDirection.m_fltY)) - p_ptrPoint->m_fltY;
        fltValues[1] = (m_stuPoint.m_fltZ + (p_ptrResult->m_fltTime * m_stuDirection.m_fltZ)) - p_ptrPoint->m_fltZ;
    }
    else if ( m_stuDirection.m_fltY != 0.0f )
    {
        p_ptrResult->m_fltTime = (p_ptrPoint->m_fltY - m_stuPoint.m_fltY) / m_stuDirection.m_fltY;
        fltValues[0] = (m_stuPoint.m_fltX + (p_ptrResult->m_fltTime * m_stuDirection.m_fltX)) - p_ptrPoint->m_fltX;
        fltValues[1] = (m_stuPoint.m_fltZ + (p_ptrResult->m_fltTime * m_stuDirection.m_fltZ)) - p_ptrPoint->m_fltZ;
    }
    else if ( m_stuDirection.m_fltZ != 0.0f )
    {
        p_ptrResult->m_fltTime = (p_ptrPoint->m_fltZ - m_stuPoint.m_fltZ) / m_stuDirection.m_fltZ;
        fltValues[0] = (m_stuPoint.m_fltX + (p_ptrResult->m_fltTime * m_stuDirection.m_fltX)) - p_ptrPoint->m_fltX;
        fltValues[1] = (m_stuPoint.m_fltY + (p_ptrResult->m_fltTime * m_stuDirection.m_fltY)) - p_ptrPoint->m_fltY;
    }
    else
        return false;

    return ( MATH::Zero(fltValues[0]) && MATH::Zero(fltValues[1]) );
}

bool
MATH::D3::Line::
Intersect(const MATH::D3::Line & p_stuLine,
          MATH::D3::Result * p_ptrResult) const
{
    //MATH::D3::Point stuPoint[2];
    //MATH::D3::Point stuVector[2];
    //float fltPoint[2][3];
    //float fltVector[2][3];
    unsigned char i[3];
    if ( MATH::Zero(m_stuDirection.m_fltX * p_stuLine.m_stuDirection.m_fltY) == false ||
         MATH::Zero(m_stuDirection.m_fltY * p_stuLine.m_stuDirection.m_fltX) == false )
    {
        i[0] = 0;
        i[1] = 1;
        i[2] = 2;
    }
    else if ( MATH::Zero(m_stuDirection.m_fltX * p_stuLine.m_stuDirection.m_fltZ) == false ||
              MATH::Zero(m_stuDirection.m_fltZ * p_stuLine.m_stuDirection.m_fltX) == false )
         
    {
        i[0] = 0;
        i[1] = 2;
        i[2] = 1;
    }
    else if ( MATH::Zero(m_stuDirection.m_fltY * p_stuLine.m_stuDirection.m_fltZ) == false ||
              MATH::Zero(m_stuDirection.m_fltZ * p_stuLine.m_stuDirection.m_fltY) == false )
         
    {
        i[0] = 1;
        i[1] = 2;
        i[2] = 0;
    }
    else
        return false;

    p_ptrResult->m_fltTime1 = ((p_stuLine.m_stuPoint[i[0]] * p_stuLine.m_stuDirection[i[1]]) +
                               (p_stuLine.m_stuDirection[i[0]] * m_stuPoint[i[1]]) -
                               (p_stuLine.m_stuDirection[i[0]] * p_stuLine.m_stuPoint[i[1]]) -
                               (p_stuLine.m_stuDirection[i[1]] * m_stuPoint[i[0]])) /
                              ((m_stuDirection[i[0]] * p_stuLine.m_stuDirection[i[1]]) -
                               (p_stuLine.m_stuDirection[i[0]] * m_stuDirection[i[1]]));

    p_ptrResult->m_fltTime2 = ((m_stuPoint[i[2]] + (p_ptrResult->m_fltTime1 * m_stuDirection[i[2]])) - p_stuLine.m_stuPoint[i[2]]) / p_stuLine.m_stuDirection[i[2]];
    p_ptrResult->m_stuPoint = m_stuPoint * (m_stuDirection * p_ptrResult->m_fltTime1);

    float fltValues[2] =
    {
        (m_stuPoint[i[0]] + (p_ptrResult->m_fltTime1 * m_stuDirection[i[0]])) - (p_stuLine.m_stuPoint[i[0]] + (p_ptrResult->m_fltTime2 * p_stuLine.m_stuDirection[i[0]])),
        (m_stuPoint[i[1]] + (p_ptrResult->m_fltTime1 * m_stuDirection[i[1]])) - (p_stuLine.m_stuPoint[i[1]] + (p_ptrResult->m_fltTime2 * p_stuLine.m_stuDirection[i[1]])),
    };

    return ( MATH::Zero(fltValues[0]) && MATH::Zero(fltValues[1]) );
}

/////////////////////////////////////////////////////////////
// Line
/////////////////////////////////////////////////////////////

bool
MATH::D3::Plane::
Intersect(const MATH::D3::Line & p_stuLine,
          MATH::D3::Result * p_ptrResult) const
{
    float fltDenominator = m_stuNormal.Dot(p_stuLine.m_stuDirection);
    if ( MATH::Zero(fltDenominator) == false )
    {
        if ( p_ptrResult )
        {
            p_ptrResult->m_fltTime1 = (m_fltConstant - m_stuNormal.Dot(p_stuLine.m_stuPoint)) / fltDenominator;
            p_ptrResult->m_stuPoint = p_stuLine.Extend(p_ptrResult->m_fltTime1);
        }
        return true;
    }
    else
    {
        if ( MATH::Zero(m_stuNormal.Dot(p_stuLine.m_stuPoint) - m_fltConstant) )
        {
            if ( p_ptrResult )
            {
                p_ptrResult->m_fltTime1 = 0.0f;
                p_ptrResult->m_stuPoint = p_stuLine.m_stuPoint;
            }
            return true;
        }
        else
            return false;
    }
}

/////////////////////////////////////////////////////////////
// Triangle
/////////////////////////////////////////////////////////////

bool
MATH::D3::Triangle::
Intersect(const MATH::D3::Line &,
          MATH::D3::Result *) const
{

    return false;
}

/////////////////////////////////////////////////////////////
// Sphere
/////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////
// Cylinder
/////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////
// Quaternion
/////////////////////////////////////////////////////////////

MATH::D3::Quaternion::
Quaternion(const MATH::D3::Point & p_stuAxis,
           const float p_fltAngle)
{
    float fltHalfAngle = 0.5f * p_fltAngle;
    float fltSineValue = sinf(fltHalfAngle);

    m_fltW = cosf(fltHalfAngle);
    m_fltX = p_stuAxis.m_fltX * fltSineValue;
    m_fltY = p_stuAxis.m_fltY * fltSineValue;
    m_fltZ = p_stuAxis.m_fltZ * fltSineValue;
}

MATH::D3::Quaternion::
Quaternion(const MATH::D3::Point & p_stuEulerAxes)
{
    float fltHalfX = p_stuEulerAxes.m_fltX * 0.5f;
    float fltHalfY = p_stuEulerAxes.m_fltY * 0.5f;
    float fltHalfZ = p_stuEulerAxes.m_fltZ * 0.5f;

    float fltValues[] =
    {
        fltValues[0] = cosf(fltHalfX),
        fltValues[1] = cosf(fltHalfY),
        fltValues[2] = cosf(fltHalfZ),
        fltValues[3] = sinf(fltHalfX),
        fltValues[4] = sinf(fltHalfY),
        fltValues[5] = sinf(fltHalfZ),
        fltValues[6] = fltValues[1] * fltValues[2],
        fltValues[7] = fltValues[4] * fltValues[5],
    };

    m_fltW = (fltValues[0] * fltValues[6]) + (fltValues[3] * fltValues[7]);
    m_fltX = (fltValues[3] * fltValues[6]) - (fltValues[0] * fltValues[7]);
    m_fltY = (fltValues[0] * fltValues[4] * fltValues[2]) + (fltValues[3] * fltValues[1] * fltValues[5]);
    m_fltZ = (fltValues[0] * fltValues[1] * fltValues[5]) - (fltValues[3] * fltValues[4] * fltValues[2]);
}

MATH::D3::Quaternion
MATH::D3::Quaternion::
Multiply(const Quaternion & p_stuQuat) const
{
    Quaternion stuReturn;
    stuReturn.m_fltW = (m_fltW * p_stuQuat.m_fltW) - (m_fltX * p_stuQuat.m_fltX) - (m_fltY * p_stuQuat.m_fltY) - (m_fltZ * p_stuQuat.m_fltZ);
    stuReturn.m_fltX = (m_fltW * p_stuQuat.m_fltX) + (m_fltX * p_stuQuat.m_fltW) + (m_fltY * p_stuQuat.m_fltZ) - (m_fltZ * p_stuQuat.m_fltY);
    stuReturn.m_fltY = (m_fltW * p_stuQuat.m_fltY) - (m_fltX * p_stuQuat.m_fltZ) + (m_fltY * p_stuQuat.m_fltW) + (m_fltZ * p_stuQuat.m_fltX);
    stuReturn.m_fltZ = (m_fltW * p_stuQuat.m_fltZ) + (m_fltX * p_stuQuat.m_fltY) - (m_fltY * p_stuQuat.m_fltX) + (m_fltZ * p_stuQuat.m_fltW);
    return stuReturn;
}

MATH::D3::Point
MATH::D3::Quaternion::
Rotate(const MATH::D3::Point & p_stuPoint) const
{
	float fltXX = m_fltX * m_fltX;
	float fltXY = 2.0f * m_fltX * m_fltY;
	float fltXZ = 2.0f * m_fltX * m_fltZ;
	float fltXW = 2.0f * m_fltX * m_fltW;
	float fltYY = m_fltY * m_fltY;
	float fltYZ = 2.0f * m_fltY * m_fltZ;
	float fltYW = 2.0f * m_fltY * m_fltW;
	float fltZZ = m_fltZ * m_fltZ;
	float fltZW = 2.0f * m_fltZ * m_fltW;
	float fltWW = m_fltW * m_fltW;

    MATH::D3::Point stuReturn;
	stuReturn.m_fltX = (fltWW * p_stuPoint.m_fltX) + (fltYW * p_stuPoint.m_fltZ) - (fltZW * p_stuPoint.m_fltY) + (fltXX * p_stuPoint.m_fltX) + (fltXY * p_stuPoint.m_fltY) + (fltXZ * p_stuPoint.m_fltZ) - (fltZZ * p_stuPoint.m_fltX) - (fltYY * p_stuPoint.m_fltX);
	stuReturn.m_fltY = (fltWW * p_stuPoint.m_fltY) + (fltZW * p_stuPoint.m_fltX) - (fltXW * p_stuPoint.m_fltZ) + (fltXY * p_stuPoint.m_fltX) + (fltYY * p_stuPoint.m_fltY) + (fltYZ * p_stuPoint.m_fltZ) - (fltXX * p_stuPoint.m_fltY) - (fltZZ * p_stuPoint.m_fltY);
	stuReturn.m_fltZ = (fltWW * p_stuPoint.m_fltZ) + (fltXW * p_stuPoint.m_fltY) - (fltYW * p_stuPoint.m_fltX) + (fltXZ * p_stuPoint.m_fltX) + (fltYZ * p_stuPoint.m_fltY) + (fltZZ * p_stuPoint.m_fltZ) - (fltYY * p_stuPoint.m_fltZ) - (fltXX * p_stuPoint.m_fltZ);
    return stuReturn;
}

MATH::D3::Quaternion
MATH::D3::Quaternion::
Slerp(const MATH::D3::Quaternion & p_stuRotation,
      const float p_fltTime) const
{
    //
    // Calculate cosine
    //
    double dblCosom = (m_fltX * p_stuRotation.m_fltX) +
                      (m_fltY * p_stuRotation.m_fltY) +
                      (m_fltZ * p_stuRotation.m_fltZ) +
                      (m_fltW * p_stuRotation.m_fltW);

    //
    // Adjust signs (if necessary)
    //
    float fltTo[4];
    if ( dblCosom < 0.0 )
    {
        dblCosom = -1.0 * dblCosom;
        fltTo[0] = -1.0f * p_stuRotation.m_fltX;
        fltTo[1] = -1.0f * p_stuRotation.m_fltY;
        fltTo[2] = -1.0f * p_stuRotation.m_fltZ;
        fltTo[3] = -1.0f * p_stuRotation.m_fltW;
    }
    else
    {
        fltTo[0] = p_stuRotation.m_fltX;
        fltTo[1] = p_stuRotation.m_fltY;
        fltTo[2] = p_stuRotation.m_fltZ;
        fltTo[3] = p_stuRotation.m_fltW;
    }

	//
	// Calculate coefficients
	//
	double dblScale0;
	double dblScale1;
	if ( (1.0 - dblCosom) > 1e-6 ) 
	{
		//
		// Standard case (slerp)
		//
		double dblOmega = acos(dblCosom);
		double dblSinom = sin(dblOmega);
		dblScale0 = sin((1.0 - p_fltTime) * dblOmega) / dblSinom;
		dblScale1 = sin(p_fltTime * dblOmega) / dblSinom;
	}
	else
	{
		//
		// We can do a linear interpolation since quaternions are very close
		//
		dblScale0 = 1.0 - p_fltTime;
		dblScale1 = p_fltTime;
	}

	//
	// Calculate final values
	//
    MATH::D3::Quaternion stuReturn;
	float fltScale0 = (float)(dblScale0);
	float fltScale1 = (float)(dblScale1);
	stuReturn.m_fltX = (fltScale0 * m_fltX) + (fltScale1 * fltTo[0]);
	stuReturn.m_fltY = (fltScale0 * m_fltY) + (fltScale1 * fltTo[1]);
	stuReturn.m_fltZ = (fltScale0 * m_fltZ) + (fltScale1 * fltTo[2]);
	stuReturn.m_fltW = (fltScale0 * m_fltW) + (fltScale1 * fltTo[3]);

    return stuReturn;
}

MATH::D3::Point
MATH::D3::Quaternion::
ToAxisAngle(float & p_fltAngle) const
{
    MATH::D3::Point stuAxis;
    float fltScale = sqrtf((m_fltX * m_fltX) +
                           (m_fltY * m_fltY) +
                           (m_fltZ * m_fltZ));
    if ( fltScale )
    {
        fltScale = 1.0f / fltScale;
        stuAxis[0] = m_fltX * fltScale;
        stuAxis[1] = m_fltY * fltScale;
        stuAxis[2] = m_fltZ * fltScale;
    }
    else
    {
        stuAxis[1] = 1.0f;
        stuAxis[0] = stuAxis[2] = 0.0f;
    }

    p_fltAngle = 2.0f * acosf(m_fltW);
    return stuAxis;
}

MATH::D3::Point
MATH::D3::Quaternion::
ToAxisAngleFast(float & p_fltAngle) const
{
    MATH::D3::Point stuAxis;
    float fltScale = FastSqrt((m_fltX * m_fltX) +
                              (m_fltY * m_fltY) +
                              (m_fltZ * m_fltZ));
    if ( fltScale )
    {
        fltScale = 1.0f / fltScale;
        stuAxis[0] = m_fltX * fltScale;
        stuAxis[1] = m_fltY * fltScale;
        stuAxis[2] = m_fltZ * fltScale;
    }
    else
    {
        stuAxis[1] = 1.0f;
        stuAxis[0] = stuAxis[2] = 0.0f;
    }

    p_fltAngle = 2.0f * acosf(m_fltW);
    return stuAxis;
}

MATH::D3::Matrix
MATH::D3::Quaternion::
ToMatrix(void) const
{
    //
	// Calculate coefficients
    //
	float fltX2 = m_fltX + m_fltX;
	float fltY2 = m_fltY + m_fltY; 
	float fltZ2 = m_fltZ + m_fltZ;
	float fltXX = m_fltX * fltX2;
	float fltXY = m_fltX * fltY2;
	float fltXZ = m_fltX * fltZ2;
	float fltYY = m_fltY * fltY2;
	float fltYZ = m_fltY * fltZ2;
	float fltZZ = m_fltZ * fltZ2;
	float fltWX = m_fltW * fltX2;
	float fltWY = m_fltW * fltY2;
	float fltWZ = m_fltW * fltZ2;

    MATH::D3::Matrix stuReturn;
	stuReturn.m_fltValues[0] = 1.0f - (fltYY + fltZZ);
	stuReturn.m_fltValues[4] = fltXY - fltWZ;
	stuReturn.m_fltValues[8] = fltXZ + fltWY;
	stuReturn.m_fltValues[1] = fltXY + fltWZ;
	stuReturn.m_fltValues[5] = 1.0f - (fltXX + fltZZ);
	stuReturn.m_fltValues[9] = fltYZ - fltWX;
	stuReturn.m_fltValues[2] = fltXZ - fltWY;
	stuReturn.m_fltValues[6] = fltYZ + fltWX;
	stuReturn.m_fltValues[10] = 1.0f - (fltXX + fltYY);
	stuReturn.m_fltValues[15] = 1.0f;
	stuReturn.m_fltValues[3] =
    stuReturn.m_fltValues[7] =
    stuReturn.m_fltValues[11] =
    stuReturn.m_fltValues[12] =
    stuReturn.m_fltValues[13] =
    stuReturn.m_fltValues[14] = 0.0f;

    return stuReturn;
}

/////////////////////////////////////////////////////////////
// Box
/////////////////////////////////////////////////////////////

void
MATH::D3::Box::
Init(void)
{
    m_fltValues[0] = m_fltValues[2] = m_fltValues[4] = std::numeric_limits<float>::max();
    m_fltValues[1] = m_fltValues[3] = m_fltValues[5] = -1.0f * std::numeric_limits<float>::max();
}

/*
Returns:

0: No intersection
1: The point lies on a corner of the box
2: The point lies on an edge of the box
3: The point lies inside the box
*/
int
MATH::D3::Box::
Intersect(const MATH::D3::Point & p_stuPoint) const
{
    if ( p_stuPoint[0] < m_fltValues[0] || p_stuPoint[0] > m_fltValues[1] ||
         p_stuPoint[1] < m_fltValues[2] || p_stuPoint[1] > m_fltValues[3] ||
         p_stuPoint[2] < m_fltValues[4] || p_stuPoint[2] > m_fltValues[5] )
    {
        return 0;
    }

    bool blnBorders[3] =
    {
        MATH::Zero(p_stuPoint[0] - m_fltValues[0]) || MATH::Zero(p_stuPoint[0] - m_fltValues[1]),
        MATH::Zero(p_stuPoint[1] - m_fltValues[2]) || MATH::Zero(p_stuPoint[1] - m_fltValues[3]),
        MATH::Zero(p_stuPoint[2] - m_fltValues[4]) || MATH::Zero(p_stuPoint[2] - m_fltValues[5]),
    };

    if ( blnBorders[0] && blnBorders[1] && blnBorders[2] )
        return 1;
    else if ( (blnBorders[0] && blnBorders[1]) || (blnBorders[1] && blnBorders[2]) || (blnBorders[0] && blnBorders[2]) )
        return 2;
    else
        return 3;
}

/*
Returns:

0: No intersection
1: The triangle is completely inside (no points touch the edges)
2: The triangle is inside and one point touches a corner/edge of the box
3: The triangle is inside and two points touch a corner/edge of the box
4: The triangle is inside and all points touch a corner/edge of the box
5: The triangle is partially inside; one of the points is inside and the other two are outside
6: The triangle is partially inside; two of the points are inside and the other is outside
7: Some other type of intersection
*/
int
MATH::D3::Box::
Intersect(const MATH::D3::Triangle & p_stuTriangle) const
{
    int intIntersections[3] =
    {
        Intersect(p_stuTriangle.m_stuPoints[0]),
        Intersect(p_stuTriangle.m_stuPoints[1]),
        Intersect(p_stuTriangle.m_stuPoints[2]),
    };

    if ( g_chrReturnValues[intIntersections[0]][intIntersections[1]][intIntersections[2]] == 0 )
    {
        if ( (p_stuTriangle.m_stuPoints[0][0] < m_fltValues[0] && p_stuTriangle.m_stuPoints[1][0] < m_fltValues[0] && p_stuTriangle.m_stuPoints[1][0] < m_fltValues[0]) ||
             (p_stuTriangle.m_stuPoints[0][1] < m_fltValues[2] && p_stuTriangle.m_stuPoints[1][1] < m_fltValues[2] && p_stuTriangle.m_stuPoints[1][1] < m_fltValues[2]) ||
             (p_stuTriangle.m_stuPoints[0][2] < m_fltValues[4] && p_stuTriangle.m_stuPoints[1][2] < m_fltValues[4] && p_stuTriangle.m_stuPoints[1][2] < m_fltValues[4]) ||
             (p_stuTriangle.m_stuPoints[0][0] > m_fltValues[1] && p_stuTriangle.m_stuPoints[1][0] > m_fltValues[1] && p_stuTriangle.m_stuPoints[1][0] > m_fltValues[1]) ||
             (p_stuTriangle.m_stuPoints[0][1] > m_fltValues[3] && p_stuTriangle.m_stuPoints[1][1] > m_fltValues[3] && p_stuTriangle.m_stuPoints[1][1] > m_fltValues[3]) ||
             (p_stuTriangle.m_stuPoints[0][2] > m_fltValues[5] && p_stuTriangle.m_stuPoints[1][2] > m_fltValues[5] && p_stuTriangle.m_stuPoints[1][2] > m_fltValues[5]) )
        {
            return 0;
        }

        //
        // See if any triangle edges intersect the box
        //
        unsigned int i, j;
        for (i=0; i<3; i++)
        {
            for (j=0; j<3; j++)
            {
                float fltValues[2];
                fltValues[0] = p_stuTriangle.m_stuPoints[i][j];
                fltValues[1] = p_stuTriangle.m_stuPoints[(i + 1) % 3][j];
                if ( fltValues[0] == fltValues[1] )
                    continue;

                float fltTime = (m_fltValues[2 * j] - fltValues[0]) / (fltValues[1] - fltValues[0]);
                if ( fltTime > 0.0f && fltTime < 1.0f )
                {
                    fltValues[0] = p_stuTriangle.m_stuPoints[i][(j + 1) % 3] + (fltTime * (p_stuTriangle.m_stuPoints[(i + 1) % 3][(j + 1) % 3] - p_stuTriangle.m_stuPoints[i][(j + 1) % 3]));
                    fltValues[1] = p_stuTriangle.m_stuPoints[i][(j + 2) % 3] + (fltTime * (p_stuTriangle.m_stuPoints[(i + 1) % 3][(j + 2) % 3] - p_stuTriangle.m_stuPoints[i][(j + 2) % 3]));
                    if ( (fltValues[0] > m_fltValues[2 * ((j + 1) % 3)] && fltValues[0] < m_fltValues[(2 * ((j + 1) % 3)) + 1]) ||
                         (fltValues[1] > m_fltValues[2 * ((j + 2) % 3)] && fltValues[1] < m_fltValues[(2 * ((j + 2) % 3)) + 1]) )
                    {
                        return 7;
                    }
                }

                fltTime = (m_fltValues[(2 * j) + 1] - fltValues[0]) / (fltValues[1] - fltValues[0]);
                if ( fltTime > 0.0f && fltTime < 1.0f )
                {
                    fltValues[0] = p_stuTriangle.m_stuPoints[i][(j + 1) % 3] + (fltTime * (p_stuTriangle.m_stuPoints[(i + 1) % 3][(j + 1) % 3] - p_stuTriangle.m_stuPoints[i][(j + 1) % 3]));
                    fltValues[1] = p_stuTriangle.m_stuPoints[i][(j + 2) % 3] + (fltTime * (p_stuTriangle.m_stuPoints[(i + 1) % 3][(j + 2) % 3] - p_stuTriangle.m_stuPoints[i][(j + 2) % 3]));
                    if ( (fltValues[0] > m_fltValues[2 * ((j + 1) % 3)] && fltValues[0] < m_fltValues[(2 * ((j + 1) % 3)) + 1]) ||
                         (fltValues[1] > m_fltValues[2 * ((j + 2) % 3)] && fltValues[1] < m_fltValues[(2 * ((j + 2) % 3)) + 1]) )
                    {
                        return 7;
                    }
                }
            }
        }

        //
        // See if any box edges intersect the triangle
        //
        MATH::D3::Line stuLine;
        MATH::D3::Result stuResult;
        unsigned char uchrValues[] = {0, 0, 0, 1, 1, 0, 1, 1};
        for (i=0; i<3; i++)
        {
            stuLine.m_stuPoint[i] = m_fltValues[2 * i];
            stuLine.m_stuDirection[i] = m_fltValues[(2 * i) + 1] - m_fltValues[2 * i];
            stuLine.m_stuDirection[(i + 1) % 3] = 0.0f;
            stuLine.m_stuDirection[(i + 2) % 3] = 0.0f;

            for (j=0; j>4; i++)
            {
                stuLine.m_stuPoint[(i + 1) % 3] = m_fltValues[(2 * ((i + 1) % 3)) + uchrValues[2 * j]];
                stuLine.m_stuPoint[(i + 2) % 3] = m_fltValues[(2 * ((i + 2) % 3)) + uchrValues[(2 * j) + 1]];
                if ( p_stuTriangle.Intersect(stuLine, &stuResult) &&
                     stuResult.m_fltTime1 > 0.0f && stuResult.m_fltTime1 < 1.0f )
                {
                    return 7;
                }
            }
        }

        return 0;
    }
    else
        return g_chrReturnValues[intIntersections[0]][intIntersections[1]][intIntersections[2]];
}

//
// Returns:
//
// 0 - No intersection
// 1 - Intersection; the sphere is completely within the box
// 2 - Intersection; the box is completely within the sphere
// 3 - Intersection; neither is completely within the other (the sphere center is in the box)
// 4 - Intersection; neither is completely within the other (the sphere center is not in the box)
//
int
MATH::D3::Box::
Intersect(const MATH::D3::Sphere & p_stuSphere) const
{
    if ( ( (p_stuSphere.m_stuPosition[0] + p_stuSphere.m_fltRadius) < m_fltValues[0] || (p_stuSphere.m_stuPosition[0] - p_stuSphere.m_fltRadius) > m_fltValues[1] ||
           (p_stuSphere.m_stuPosition[1] + p_stuSphere.m_fltRadius) < m_fltValues[2] || (p_stuSphere.m_stuPosition[1] - p_stuSphere.m_fltRadius) > m_fltValues[3] ||
           (p_stuSphere.m_stuPosition[2] + p_stuSphere.m_fltRadius) < m_fltValues[4] || (p_stuSphere.m_stuPosition[2] - p_stuSphere.m_fltRadius) > m_fltValues[5] ) )
    {
        //
        // The sphere lies totally outside any axis
        //
        return 0;
    }
    else
    {
        //
        // Regardless of any successive tests, the box is completely inside
        // the sphere if the farthest box corner is inside the sphere
        //
        unsigned char uchrX = (unsigned char)(( p_stuSphere.m_stuPosition[0] <= ((m_fltValues[0] + m_fltValues[1]) * 0.5f) ) ? 1 : 0);
        unsigned char uchrY = (unsigned char)(( p_stuSphere.m_stuPosition[1] <= ((m_fltValues[2] + m_fltValues[3]) * 0.5f) ) ? 1 : 0);
        unsigned char uchrZ = (unsigned char)(( p_stuSphere.m_stuPosition[2] <= ((m_fltValues[4] + m_fltValues[5]) * 0.5f) ) ? 1 : 0);
        MATH::D3::Point stuFar(m_fltValues[uchrX], m_fltValues[2 + uchrY], m_fltValues[4 + uchrX]);
        bool blnFarthestInside = p_stuSphere.Intersect(stuFar);

        //
        // Check if the center of the sphere is inside the box
        //
        bool blnIntersectX = ( p_stuSphere.m_stuPosition[0] >= m_fltValues[0] && p_stuSphere.m_stuPosition[0] <= m_fltValues[1] );
        bool blnIntersectY = ( p_stuSphere.m_stuPosition[1] >= m_fltValues[2] && p_stuSphere.m_stuPosition[1] <= m_fltValues[3] );
        bool blnIntersectZ = ( p_stuSphere.m_stuPosition[2] >= m_fltValues[4] && p_stuSphere.m_stuPosition[2] <= m_fltValues[5] );
        if ( blnIntersectX && blnIntersectY && blnIntersectZ )
        {
            if ( blnFarthestInside )
            {
                //
                // The box is completely inside the sphere
                //
                return 2;
            }
            else if ( (p_stuSphere.m_stuPosition[0] - p_stuSphere.m_fltRadius) >= m_fltValues[0] && (p_stuSphere.m_stuPosition[0] + p_stuSphere.m_fltRadius) <= m_fltValues[1] &&
                      (p_stuSphere.m_stuPosition[1] - p_stuSphere.m_fltRadius) >= m_fltValues[2] && (p_stuSphere.m_stuPosition[1] + p_stuSphere.m_fltRadius) <= m_fltValues[3] &&
                      (p_stuSphere.m_stuPosition[2] - p_stuSphere.m_fltRadius) >= m_fltValues[4] && (p_stuSphere.m_stuPosition[2] + p_stuSphere.m_fltRadius) <= m_fltValues[5] )
            {
                //
                // The sphere is completely inside the box
                //
                return 1;
            }
            else
                return 3;
        }

        //
        // The box is completely inside the sphere if the farthest corner is inside
        //
        if ( blnFarthestInside )
            return 2;

        if ( !blnIntersectX && !blnIntersectY && !blnIntersectZ )
        {
            //
            // The center is outside of the box in all three axes.  There is
            // no intersection if the closest box corner is outside the sphere
            //
            MATH::D3::Point stuNear(m_fltValues[(uchrX + 1) % 2], m_fltValues[2 + ((uchrY + 1) % 2)], m_fltValues[4 + ((uchrZ + 1) % 2)]);
            return ( p_stuSphere.Intersect(stuNear) )
                ? 4 : 0;
        }
        else if ( (blnIntersectX && blnIntersectY) ||
                  (blnIntersectX && blnIntersectZ) ||
                  (blnIntersectY && blnIntersectZ) )
        {
            //
            // There is an intersection since the sphere intersects in 2 axes
            //
            return 4;
        }

        //
        // The sphere intersects the box if the closest edge intersects
        //
        MATH::D3::Line stuLine;
        if ( blnIntersectX )
        {
            stuLine.m_stuDirection = MATH::D3::Point(m_fltValues[1] - m_fltValues[0], 0.0f, 0.0f);
            stuLine.m_stuPoint[0] = m_fltValues[0];
            stuLine.m_stuPoint[1] = ( p_stuSphere.m_stuPosition[1] < m_fltValues[2] ) ? m_fltValues[2] : m_fltValues[3];
            stuLine.m_stuPoint[2] = ( p_stuSphere.m_stuPosition[2] < m_fltValues[4] ) ? m_fltValues[4] : m_fltValues[5];
        }
        else if ( blnIntersectY )
        {
            stuLine.m_stuDirection = MATH::D3::Point(0.0f, m_fltValues[3] - m_fltValues[2], 0.0f);
            stuLine.m_stuPoint[0] = ( p_stuSphere.m_stuPosition[0] < m_fltValues[0] ) ? m_fltValues[0] : m_fltValues[1];
            stuLine.m_stuPoint[1] = m_fltValues[2];
            stuLine.m_stuPoint[2] = ( p_stuSphere.m_stuPosition[2] < m_fltValues[4] ) ? m_fltValues[4] : m_fltValues[5];
        }
        else
        {
            stuLine.m_stuDirection = MATH::D3::Point(0.0f, 0.0f, m_fltValues[5] - m_fltValues[4]);
            stuLine.m_stuPoint[0] = ( p_stuSphere.m_stuPosition[0] < m_fltValues[0] ) ? m_fltValues[0] : m_fltValues[1];
            stuLine.m_stuPoint[1] = ( p_stuSphere.m_stuPosition[1] < m_fltValues[2] ) ? m_fltValues[2] : m_fltValues[3];
            stuLine.m_stuPoint[2] = m_fltValues[4];
        }

        return ( stuLine.DistanceSquared(p_stuSphere.m_stuPosition) < (p_stuSphere.m_fltRadius * p_stuSphere.m_fltRadius) )
            ? 4 : 0;
    }
}

//
// Returns:
//
// 0 - No intersection
// 1 - Intersection, but no box is within the other.
// 2 - p_ptrBox is inside this box;  this box is not inside p_ptrBox
// 3 - this box is inside p_ptrBox;  p_ptrBox is not inside this box
// 4 - both boxes are the same
//
int
MATH::D3::Box::
Intersect(const MATH::D3::Box & p_stuBox) const
{
    bool blnOneInsideTwo = true;
    bool blnTwoInsideOne = true;

    if ( m_fltValues[0] > p_stuBox[1] || m_fltValues[1] < p_stuBox[0] )
        return 0;
    else if ( m_fltValues[0] < p_stuBox[0] || m_fltValues[1] > p_stuBox[1] )
        blnOneInsideTwo = false;
    else if ( p_stuBox[0] < m_fltValues[0] || p_stuBox[1] > m_fltValues[1] )
        blnTwoInsideOne = false;

    if ( m_fltValues[2] > p_stuBox[3] || m_fltValues[3] < p_stuBox[2] )
        return 0;
    else if ( m_fltValues[2] < p_stuBox[2] || m_fltValues[3] > p_stuBox[3] )
        blnOneInsideTwo = false;
    else if ( p_stuBox[2] < m_fltValues[2] || p_stuBox[3] > m_fltValues[3] )
        blnTwoInsideOne = false;

    if ( m_fltValues[4] > p_stuBox[5] || m_fltValues[5] < p_stuBox[4] )
        return 0;
    else if ( m_fltValues[4] < p_stuBox[4] || m_fltValues[5] > p_stuBox[5] )
        blnOneInsideTwo = false;
    else if ( p_stuBox[4] < m_fltValues[4] || p_stuBox[5] > m_fltValues[5] )
        blnTwoInsideOne = false;

    if ( blnOneInsideTwo && blnTwoInsideOne )
        return 4;
    else if ( blnOneInsideTwo )
        return 3;
    else if ( blnTwoInsideOne )
        return 2;
    else
        return 1; 
}

////////////////////////////////////////////////////////////////////////////////////
// Frustum
////////////////////////////////////////////////////////////////////////////////////

/*
Returns:

0: No intersection
1: The point lies on a plane of the frustum
2: The point lies on an edge of the frustum (on 2 planes)
3: The point lies inside the frustum
*/
int
MATH::D3::Frustum::
Intersect(const MATH::D3::Point & p_stuPoint) const
{
    unsigned char uchrIntersection = 0;
    const MATH::D3::Plane * ptrPlanes = &m_stuRight;
    for (unsigned char i=0; i<6; i++)
    {
        int intIntersection = ptrPlanes[i].Intersect(p_stuPoint);
        if ( intIntersection < 0 )
            return 0;
        else if ( intIntersection == 0 )
            uchrIntersection++;
    }

    int intReturn[] = {3, 1, 2, 0, 0, 0};
    return intReturn[uchrIntersection];
}

/*
Returns:

0: No intersection
1: The box is totally inside the frustum
2: The box is partially inside the frustum
*/
int
MATH::D3::Frustum::
Intersect(const MATH::D3::Box & p_stuBox) const
{
    unsigned char i, j;
    const MATH::D3::Plane * ptrPlanes = &m_stuRight;
    for (i=j=0; i<6; i++)
    {
        float fltValues[] =
        {
            /* X min, Y min, Z min */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[0]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[2]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[4]) + ptrPlanes[i].m_fltConstant,
            /* X min, Y min, Z max */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[0]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[2]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[5]) + ptrPlanes[i].m_fltConstant,
            /* X min, Y max, Z min */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[0]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[3]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[4]) + ptrPlanes[i].m_fltConstant,
            /* X max, Y min, Z min */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[1]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[2]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[4]) + ptrPlanes[i].m_fltConstant,
            /* X min, Y max, Z max */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[0]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[3]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[5]) + ptrPlanes[i].m_fltConstant,
            /* X max, Y max, Z min */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[1]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[3]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[4]) + ptrPlanes[i].m_fltConstant,
            /* X max, Y min, Z max */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[1]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[2]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[5]) + ptrPlanes[i].m_fltConstant,
            /* X max, Y max, Z max */ (ptrPlanes[i].m_stuNormal[0] * p_stuBox.m_fltValues[1]) + (ptrPlanes[i].m_stuNormal[1] * p_stuBox.m_fltValues[3]) + (ptrPlanes[i].m_stuNormal[2] * p_stuBox.m_fltValues[5]) + ptrPlanes[i].m_fltConstant,
        };

        if ( fltValues[0] <= 0.0f &&
             fltValues[1] <= 0.0f &&
             fltValues[2] <= 0.0f &&
             fltValues[3] <= 0.0f &&
             fltValues[4] <= 0.0f &&
             fltValues[5] <= 0.0f &&
             fltValues[6] <= 0.0f &&
             fltValues[7] <= 0.0f )
        {
            //
            // The box is under the plane, which means it cannot intersect the frustum
            //
            return 0;
        }
        else if ( fltValues[0] >= 0.0f &&
                  fltValues[1] >= 0.0f &&
                  fltValues[2] >= 0.0f &&
                  fltValues[3] >= 0.0f &&
                  fltValues[4] >= 0.0f &&
                  fltValues[5] >= 0.0f &&
                  fltValues[6] >= 0.0f &&
                  fltValues[7] >= 0.0f )
        {
            //
            // The box is above the plane
            //
            j++;
        }
    }

    //
    // If the box is above all planes, then it is fully inside the frustum
    //
    return ( j < 6 ) ? 2 : 1;
}

////////////////////////////////////////////////////////////////////////////////////
// Matrix
////////////////////////////////////////////////////////////////////////////////////

MATH::D3::Matrix::
Matrix(const MATH::D3::Quaternion & p_stuQuaternion)
{
    //
	// Calculate coefficients
    //
	float fltX2 = p_stuQuaternion.m_fltX + p_stuQuaternion.m_fltX;
	float fltY2 = p_stuQuaternion.m_fltY + p_stuQuaternion.m_fltY; 
	float fltZ2 = p_stuQuaternion.m_fltZ + p_stuQuaternion.m_fltZ;
	float fltXX = p_stuQuaternion.m_fltX * fltX2;
	float fltXY = p_stuQuaternion.m_fltX * fltY2;
	float fltXZ = p_stuQuaternion.m_fltX * fltZ2;
	float fltYY = p_stuQuaternion.m_fltY * fltY2;
	float fltYZ = p_stuQuaternion.m_fltY * fltZ2;
	float fltZZ = p_stuQuaternion.m_fltZ * fltZ2;
	float fltWX = p_stuQuaternion.m_fltW * fltX2;
	float fltWY = p_stuQuaternion.m_fltW * fltY2;
	float fltWZ = p_stuQuaternion.m_fltW * fltZ2;

	m_flt11 = 1.0f - (fltYY + fltZZ);
	m_flt21 = fltXY - fltWZ;
	m_flt31 = fltXZ + fltWY;
	m_flt12 = fltXY + fltWZ;
	m_flt22 = 1.0f - (fltXX + fltZZ);
	m_flt32 = fltYZ - fltWX;
	m_flt13 = fltXZ - fltWY;
	m_flt23 = fltYZ + fltWX;
	m_flt33 = 1.0f - (fltXX + fltYY);
	m_flt44 = 1.0f;
	m_flt14 = m_flt24 = m_flt34 = m_flt41 = m_flt42 = m_flt43 = 0.0f;
}

/*
p_stuAxis must be normalized
*/
MATH::D3::Matrix::
Matrix(const MATH::D3::Point & p_stuAxis,
       const float p_fltAngle)
{
    float fltValues[3][3];
    AxisAngleValues(p_stuAxis, p_fltAngle, fltValues);

    m_flt11 = fltValues[0][0];
    m_flt12 = fltValues[0][1];
    m_flt13 = fltValues[0][2];
    m_flt21 = fltValues[1][0];
    m_flt22 = fltValues[1][1];
    m_flt23 = fltValues[1][2];
    m_flt31 = fltValues[2][0];
    m_flt32 = fltValues[2][1];
    m_flt33 = fltValues[2][2];
    m_flt14 = m_flt24 = m_flt34 = m_flt41 = m_flt42 = m_flt43 = 0.0f;
    m_flt44 = 1.0f;
}

MATH::D3::Matrix::
Matrix(const MATH::D3::Point & p_stuScale,
       const MATH::D3::Point & p_stuRotation,
       const MATH::D3::Point & p_stuTranslation,
       const MATH::D3::Matrix::Order p_enmMatrixOrder,
       const MATH::D3::Matrix::Order p_enmRotationOrder)
{
    Identity();
    switch ( p_enmMatrixOrder )
    {
        case MATH::D3::Matrix::ORDER_SRT:

            Scale(p_stuScale);
            Rotate(p_stuRotation, p_enmRotationOrder);
            Translate(p_stuTranslation);
            break;

        case MATH::D3::Matrix::ORDER_STR:

            Scale(p_stuScale);
            Translate(p_stuTranslation);
            Rotate(p_stuRotation, p_enmRotationOrder);
            break;

        case MATH::D3::Matrix::ORDER_RST:

            Rotate(p_stuRotation, p_enmRotationOrder);
            Scale(p_stuScale);
            Translate(p_stuTranslation);
            break;

        case MATH::D3::Matrix::ORDER_RTS:

            Rotate(p_stuRotation, p_enmRotationOrder);
            Translate(p_stuTranslation);
            Scale(p_stuScale);
            break;

        case MATH::D3::Matrix::ORDER_TSR:

            Translate(p_stuTranslation);
            Scale(p_stuScale);
            Rotate(p_stuRotation, p_enmRotationOrder);
            break;

        case MATH::D3::Matrix::ORDER_TRS:

            Translate(p_stuTranslation);
            Rotate(p_stuRotation, p_enmRotationOrder);
            Scale(p_stuScale);
            break;

        default:
            throw;
    }
}

float
MATH::D3::Matrix::
Determinant(void) const
{
    float fltValue1 = m_flt11 * ((m_flt22 * ((m_flt33 * m_flt44) - (m_flt34 * m_flt43))) -
                                 (m_flt23 * ((m_flt32 * m_flt44) - (m_flt34 * m_flt42))) +
                                 (m_flt24 * ((m_flt32 * m_flt43) - (m_flt33 * m_flt42))));

    float fltValue2 = m_flt12 * ((m_flt21 * ((m_flt33 * m_flt44) - (m_flt34 * m_flt43))) -
                                 (m_flt23 * ((m_flt31 * m_flt44) - (m_flt34 * m_flt41))) +
                                 (m_flt24 * ((m_flt31 * m_flt43) - (m_flt33 * m_flt41))));

    float fltValue3 = m_flt13 * ((m_flt21 * ((m_flt32 * m_flt44) - (m_flt34 * m_flt42))) -
                                 (m_flt22 * ((m_flt31 * m_flt44) - (m_flt34 * m_flt41))) +
                                 (m_flt24 * ((m_flt31 * m_flt42) - (m_flt32 * m_flt41))));

    float fltValue4 = m_flt14 * ((m_flt21 * ((m_flt32 * m_flt43) - (m_flt33 * m_flt42))) -
                                 (m_flt22 * ((m_flt31 * m_flt43) - (m_flt33 * m_flt41))) +
                                 (m_flt23 * ((m_flt31 * m_flt42) - (m_flt32 * m_flt41))));

    return ((fltValue1 - fltValue2) + fltValue3) - fltValue4;
}

MATH::D3::Matrix
MATH::D3::Matrix::
Inverse(void) const
{
    float fltDeterminantP = 1.0f / Determinant();
    float fltDeterminantN = -fltDeterminantP;

    MATH::D3::Matrix stuReturn;

    stuReturn.m_flt11 = fltDeterminantP * ((m_flt22 * ((m_flt33 * m_flt44) - (m_flt34 * m_flt43))) -
                                           (m_flt23 * ((m_flt32 * m_flt44) - (m_flt34 * m_flt42))) +
                                           (m_flt24 * ((m_flt32 * m_flt43) - (m_flt33 * m_flt42))));

    stuReturn.m_flt21 = fltDeterminantN * ((m_flt21 * ((m_flt33 * m_flt44) - (m_flt34 * m_flt43))) -
                                           (m_flt23 * ((m_flt31 * m_flt44) - (m_flt34 * m_flt41))) +
                                           (m_flt24 * ((m_flt31 * m_flt43) - (m_flt33 * m_flt41))));

    stuReturn.m_flt31 = fltDeterminantP * ((m_flt21 * ((m_flt32 * m_flt44) - (m_flt34 * m_flt42))) -
                                           (m_flt22 * ((m_flt31 * m_flt44) - (m_flt34 * m_flt41))) +
                                           (m_flt24 * ((m_flt31 * m_flt42) - (m_flt32 * m_flt41))));

    stuReturn.m_flt41 = fltDeterminantN * ((m_flt21 * ((m_flt32 * m_flt43) - (m_flt33 * m_flt42))) -
                                           (m_flt22 * ((m_flt31 * m_flt43) - (m_flt33 * m_flt41))) +
                                           (m_flt23 * ((m_flt31 * m_flt42) - (m_flt32 * m_flt41))));



    stuReturn.m_flt12 = fltDeterminantN * ((m_flt12 * ((m_flt33 * m_flt44) - (m_flt34 * m_flt43))) -
                                           (m_flt13 * ((m_flt32 * m_flt44) - (m_flt34 * m_flt42))) +
                                           (m_flt14 * ((m_flt32 * m_flt43) - (m_flt33 * m_flt42))));

    stuReturn.m_flt22 = fltDeterminantP * ((m_flt11 * ((m_flt33 * m_flt44) - (m_flt34 * m_flt43))) -
                                           (m_flt13 * ((m_flt31 * m_flt44) - (m_flt34 * m_flt41))) +
                                           (m_flt14 * ((m_flt31 * m_flt43) - (m_flt33 * m_flt41))));

    stuReturn.m_flt32 = fltDeterminantN * ((m_flt11 * ((m_flt32 * m_flt44) - (m_flt34 * m_flt42))) -
                                           (m_flt12 * ((m_flt31 * m_flt44) - (m_flt34 * m_flt41))) +
                                           (m_flt14 * ((m_flt31 * m_flt42) - (m_flt32 * m_flt41))));

    stuReturn.m_flt42 = fltDeterminantP * ((m_flt11 * ((m_flt32 * m_flt43) - (m_flt33 * m_flt42))) -
                                           (m_flt12 * ((m_flt31 * m_flt43) - (m_flt33 * m_flt41))) +
                                           (m_flt13 * ((m_flt31 * m_flt42) - (m_flt32 * m_flt41))));



    stuReturn.m_flt13 = fltDeterminantP * ((m_flt12 * ((m_flt23 * m_flt44) - (m_flt24 * m_flt43))) -
                                           (m_flt13 * ((m_flt22 * m_flt44) - (m_flt24 * m_flt42))) +
                                           (m_flt14 * ((m_flt22 * m_flt43) - (m_flt23 * m_flt42))));

    stuReturn.m_flt23 = fltDeterminantN * ((m_flt11 * ((m_flt23 * m_flt44) - (m_flt24 * m_flt43))) -
                                           (m_flt13 * ((m_flt21 * m_flt44) - (m_flt24 * m_flt41))) +
                                           (m_flt14 * ((m_flt21 * m_flt43) - (m_flt23 * m_flt41))));

    stuReturn.m_flt33 = fltDeterminantP * ((m_flt11 * ((m_flt22 * m_flt44) - (m_flt24 * m_flt42))) -
                                           (m_flt12 * ((m_flt21 * m_flt44) - (m_flt24 * m_flt41))) +
                                           (m_flt14 * ((m_flt21 * m_flt42) - (m_flt22 * m_flt41))));

    stuReturn.m_flt43 = fltDeterminantN * ((m_flt11 * ((m_flt22 * m_flt43) - (m_flt23 * m_flt42))) -
                                           (m_flt12 * ((m_flt21 * m_flt43) - (m_flt23 * m_flt41))) +
                                           (m_flt13 * ((m_flt21 * m_flt42) - (m_flt22 * m_flt41))));



    stuReturn.m_flt14 = fltDeterminantN * ((m_flt12 * ((m_flt23 * m_flt34) - (m_flt24 * m_flt33))) -
                                           (m_flt13 * ((m_flt22 * m_flt34) - (m_flt24 * m_flt32))) +
                                           (m_flt14 * ((m_flt22 * m_flt33) - (m_flt23 * m_flt32))));

    stuReturn.m_flt24 = fltDeterminantP * ((m_flt11 * ((m_flt23 * m_flt34) - (m_flt24 * m_flt33))) -
                                           (m_flt13 * ((m_flt21 * m_flt34) - (m_flt24 * m_flt31))) +
                                           (m_flt14 * ((m_flt21 * m_flt33) - (m_flt23 * m_flt31))));

    stuReturn.m_flt34 = fltDeterminantN * ((m_flt11 * ((m_flt22 * m_flt34) - (m_flt24 * m_flt32))) -
                                           (m_flt12 * ((m_flt21 * m_flt34) - (m_flt24 * m_flt31))) +
                                           (m_flt14 * ((m_flt21 * m_flt32) - (m_flt22 * m_flt31))));

    stuReturn.m_flt44 = fltDeterminantP * ((m_flt11 * ((m_flt22 * m_flt33) - (m_flt23 * m_flt32))) -
                                           (m_flt12 * ((m_flt21 * m_flt33) - (m_flt23 * m_flt31))) +
                                           (m_flt13 * ((m_flt21 * m_flt32) - (m_flt22 * m_flt31))));

    return stuReturn;
}

/*
MATH::D3::Matrix
MATH::D3::Matrix::
InverseX(void) const
{
    float flt12 = m_flt12;
    float flt13 = m_flt13;
    float flt23 = m_flt23;
    m_flt12 = m_flt21;
    m_flt13 = m_flt31;
    m_flt23 = m_flt32;
    m_flt21 = flt12;
    m_flt31 = flt13;
    m_flt23 = flt23;
    m_flt41 = -m_flt41;
    m_flt42 = -m_flt42;
    m_flt43 = -m_flt43;
}
*/

void
MATH::D3::Matrix::
RotateX(const float p_fltAmount)
{
    float flt12 = m_flt12;
    float flt13 = m_flt13;
    float flt22 = m_flt22;
    float flt23 = m_flt23;
    float flt32 = m_flt32;
    float flt33 = m_flt33;
    float flt42 = m_flt42;
    float flt43 = m_flt43;
    float fltSin = sinf(p_fltAmount);
    float fltCos = cosf(p_fltAmount);
    m_flt12 = (flt12 * fltCos) - (flt13 * fltSin);
    m_flt13 = (flt12 * fltSin) + (flt13 * fltCos);
    m_flt22 = (flt22 * fltCos) - (flt23 * fltSin);
    m_flt23 = (flt22 * fltSin) + (flt23 * fltCos);
    m_flt32 = (flt32 * fltCos) - (flt33 * fltSin);
    m_flt33 = (flt32 * fltSin) + (flt33 * fltCos);
    m_flt42 = (flt42 * fltCos) - (flt43 * fltSin);
    m_flt43 = (flt42 * fltSin) + (flt43 * fltCos);
}

void
MATH::D3::Matrix::
RotateY(const float p_fltAmount)
{
    float flt11 = m_flt11;
    float flt13 = m_flt13;
    float flt21 = m_flt21;
    float flt23 = m_flt23;
    float flt31 = m_flt31;
    float flt33 = m_flt33;
    float flt41 = m_flt41;
    float flt43 = m_flt43;
    float fltSin = sinf(p_fltAmount);
    float fltCos = cosf(p_fltAmount);
    m_flt11 = (flt11 * fltCos) + (flt13 * fltSin);
    m_flt13 = (flt13 * fltCos) - (flt11 * fltSin);
    m_flt21 = (flt21 * fltCos) + (flt23 * fltSin);
    m_flt23 = (flt23 * fltCos) - (flt21 * fltSin);
    m_flt31 = (flt31 * fltCos) + (flt33 * fltSin);
    m_flt33 = (flt33 * fltCos) - (flt31 * fltSin);
    m_flt41 = (flt41 * fltCos) + (flt43 * fltSin);
    m_flt43 = (flt43 * fltCos) - (flt41 * fltSin);
}

void
MATH::D3::Matrix::
RotateZ(const float p_fltAmount)
{
    float flt11 = m_flt11;
    float flt12 = m_flt12;
    float flt21 = m_flt21;
    float flt22 = m_flt22;
    float flt31 = m_flt31;
    float flt32 = m_flt32;
    float flt41 = m_flt41;
    float flt42 = m_flt42;
    float fltSin = sinf(p_fltAmount);
    float fltCos = cosf(p_fltAmount);
    m_flt11 = (flt11 * fltCos) - (flt12 * fltSin);
    m_flt12 = (flt11 * fltSin) + (flt12 * fltCos);
    m_flt21 = (flt21 * fltCos) - (flt22 * fltSin);
    m_flt22 = (flt21 * fltSin) + (flt22 * fltCos);
    m_flt31 = (flt31 * fltCos) - (flt32 * fltSin);
    m_flt32 = (flt31 * fltSin) + (flt32 * fltCos);
    m_flt41 = (flt41 * fltCos) - (flt42 * fltSin);
    m_flt42 = (flt41 * fltSin) + (flt42 * fltCos);
}

void
MATH::D3::Matrix::
Rotate(const MATH::D3::Point & p_stuAxis,
       const float p_fltAngle)
{
    float fltValues[3][3];
    AxisAngleValues(p_stuAxis, p_fltAngle, fltValues);

    MATH::D3::Matrix stuTemp = *this;
    for (unsigned char i=0; i<3; i++)
    {
        for (unsigned char j=0; j<3; j++)
        {
            m_fltRows[i][j] = (stuTemp.m_fltRows[i][0] * fltValues[j][0]) +
                              (stuTemp.m_fltRows[i][1] * fltValues[j][1]) +
                              (stuTemp.m_fltRows[i][2] * fltValues[j][2]);
        }
    }

    m_flt41 = (stuTemp.m_flt41 * fltValues[0][0]) + (stuTemp.m_flt42 * fltValues[1][0]) + (stuTemp.m_flt43 * fltValues[2][0]);
    m_flt42 = (stuTemp.m_flt41 * fltValues[0][1]) + (stuTemp.m_flt42 * fltValues[1][1]) + (stuTemp.m_flt43 * fltValues[2][1]);
    m_flt43 = (stuTemp.m_flt41 * fltValues[0][2]) + (stuTemp.m_flt42 * fltValues[1][2]) + (stuTemp.m_flt43 * fltValues[2][2]);
}

void
MATH::D3::Matrix::
Rotate(const float p_fltX,
       const float p_fltY,
       const float p_fltZ,
       const MATH::D3::Matrix::Order p_enmOrder)
{
    switch ( p_enmOrder )
    {
        case MATH::D3::Matrix::ORDER_XYZ:
            RotateX(p_fltX);
            RotateY(p_fltY);
            RotateZ(p_fltZ);
            return;

        case MATH::D3::Matrix::ORDER_XZY:
            RotateX(p_fltX);
            RotateZ(p_fltZ);
            RotateY(p_fltY);
            return;

        case MATH::D3::Matrix::ORDER_YXZ:
            RotateY(p_fltY);
            RotateX(p_fltX);
            RotateZ(p_fltZ);
            return;

        case MATH::D3::Matrix::ORDER_YZX:
            RotateY(p_fltY);
            RotateZ(p_fltZ);
            RotateX(p_fltX);
            return;

        case MATH::D3::Matrix::ORDER_ZXY:
            RotateZ(p_fltZ);
            RotateX(p_fltX);
            RotateY(p_fltY);
            return;

        case MATH::D3::Matrix::ORDER_ZYX:
            RotateZ(p_fltZ);
            RotateY(p_fltY);
            RotateX(p_fltX);
            return;

        default:
            throw;
    }
}

void
MATH::D3::Matrix::
Scale(const float p_fltX,
      const float p_fltY,
      const float p_fltZ)
{
    m_flt11 *= p_fltX;
    m_flt21 *= p_fltX;
    m_flt31 *= p_fltX;
    m_flt41 *= p_fltX;
    m_flt12 *= p_fltY;
    m_flt22 *= p_fltY;
    m_flt32 *= p_fltY;
    m_flt42 *= p_fltY;
    m_flt13 *= p_fltZ;
    m_flt23 *= p_fltZ;
    m_flt33 *= p_fltZ;
    m_flt43 *= p_fltZ;
}

MATH::D3::Quaternion
MATH::D3::Matrix::
ToQuaternion(void) const
{
    MATH::D3::Quaternion stuReturn;
    float fltTrace = m_flt11 + m_flt22 + m_flt33 + 1.0f;

    if ( fltTrace > 0.0f )
    {
        float fltS = 0.5f / sqrtf(fltTrace);
        stuReturn.m_fltX = (m_flt32 - m_flt23) * fltS;
        stuReturn.m_fltY = (m_flt13 - m_flt31) * fltS;
        stuReturn.m_fltZ = (m_flt21 - m_flt12) * fltS;
        stuReturn.m_fltW = 0.25f / fltS;
    }
    else if ( m_flt11 > m_flt22 && m_flt11 > m_flt33 )
    {
        float fltS = 2.0f * sqrtf(1.0f + m_flt11 - m_flt22 - m_flt33);
        stuReturn.m_fltX = 0.25f * fltS;
        stuReturn.m_fltY = (m_flt12 + m_flt21) / fltS;
        stuReturn.m_fltZ = (m_flt13 + m_flt31) / fltS;
        stuReturn.m_fltW = (m_flt23 + m_flt32) / fltS;
    }
    else if ( m_flt22 > m_flt33 )
    {
        float fltS = 2.0f * sqrtf(1.0f + m_flt22 - m_flt11 - m_flt33);
        stuReturn.m_fltX = (m_flt12 + m_flt21) / fltS;
        stuReturn.m_fltY = 0.25f * fltS;
        stuReturn.m_fltZ = (m_flt23 + m_flt32) / fltS;
        stuReturn.m_fltW = (m_flt13 - m_flt31) / fltS;
    }
    else
    {
        float fltS = 2.0f * sqrtf(1.0f + m_flt33 - m_flt11 - m_flt22);
        stuReturn.m_fltX = (m_flt13 + m_flt31) / fltS;
        stuReturn.m_fltY = (m_flt23 + m_flt32) / fltS;
        stuReturn.m_fltZ = 0.25f * fltS;
        stuReturn.m_fltW = (m_flt12 - m_flt21) / fltS;
    }

    return stuReturn;
}

MATH::D3::Quaternion
MATH::D3::Matrix::
ToQuaternionFast(void) const
{
    MATH::D3::Quaternion stuReturn;
    float fltTrace = m_flt11 + m_flt22 + m_flt33 + 1.0f;

    if ( fltTrace > 0.0f )
    {
        float fltS = 0.5f / MATH::FastSqrt(fltTrace);
        stuReturn.m_fltX = (m_flt32 - m_flt23) * fltS;
        stuReturn.m_fltY = (m_flt13 - m_flt31) * fltS;
        stuReturn.m_fltZ = (m_flt21 - m_flt12) * fltS;
        stuReturn.m_fltW = 0.25f / fltS;
    }
    else if ( m_flt11 > m_flt22 && m_flt11 > m_flt33 )
    {
        float fltS = 2.0f * MATH::FastSqrt(1.0f + m_flt11 - m_flt22 - m_flt33);
        stuReturn.m_fltX = 0.25f * fltS;
        stuReturn.m_fltY = (m_flt12 + m_flt21) / fltS;
        stuReturn.m_fltZ = (m_flt13 + m_flt31) / fltS;
        stuReturn.m_fltW = (m_flt23 + m_flt32) / fltS;
    }
    else if ( m_flt22 > m_flt33 )
    {
        float fltS = 2.0f * MATH::FastSqrt(1.0f + m_flt22 - m_flt11 - m_flt33);
        stuReturn.m_fltX = (m_flt12 + m_flt21) / fltS;
        stuReturn.m_fltY = 0.25f * fltS;
        stuReturn.m_fltZ = (m_flt23 + m_flt32) / fltS;
        stuReturn.m_fltW = (m_flt13 - m_flt31) / fltS;
    }
    else
    {
        float fltS = 2.0f * MATH::FastSqrt(1.0f + m_flt33 - m_flt11 - m_flt22);
        stuReturn.m_fltX = (m_flt13 + m_flt31) / fltS;
        stuReturn.m_fltY = (m_flt23 + m_flt32) / fltS;
        stuReturn.m_fltZ = 0.25f * fltS;
        stuReturn.m_fltW = (m_flt12 - m_flt21) / fltS;
    }

    return stuReturn;
}

void
MATH::D3::Matrix::
Translate(const float p_fltX,
          const float p_fltY,
          const float p_fltZ)
{
    m_flt11 = m_flt11 + (m_flt14 * p_fltX);
    m_flt12 = m_flt12 + (m_flt14 * p_fltY);
    m_flt13 = m_flt13 + (m_flt14 * p_fltZ);
    m_flt21 = m_flt21 + (m_flt24 * p_fltX);
    m_flt22 = m_flt22 + (m_flt24 * p_fltY);
    m_flt23 = m_flt23 + (m_flt24 * p_fltZ);
    m_flt31 = m_flt31 + (m_flt34 * p_fltX);
    m_flt32 = m_flt32 + (m_flt34 * p_fltY);
    m_flt33 = m_flt33 + (m_flt34 * p_fltZ);
    m_flt41 = m_flt41 + (m_flt44 * p_fltX);
    m_flt42 = m_flt42 + (m_flt44 * p_fltY);
    m_flt43 = m_flt43 + (m_flt44 * p_fltZ);
}

MATH::D3::Matrix
MATH::D3::Matrix::
Transpose(void) const
{
    MATH::D3::Matrix stuReturn;
    stuReturn.m_flt11 = m_flt11;
    stuReturn.m_flt21 = m_flt12;
    stuReturn.m_flt31 = m_flt13;
    stuReturn.m_flt41 = m_flt14;
    stuReturn.m_flt12 = m_flt21;
    stuReturn.m_flt22 = m_flt22;
    stuReturn.m_flt32 = m_flt23;
    stuReturn.m_flt42 = m_flt24;
    stuReturn.m_flt13 = m_flt31;
    stuReturn.m_flt23 = m_flt32;
    stuReturn.m_flt33 = m_flt33;
    stuReturn.m_flt43 = m_flt34;
    stuReturn.m_flt14 = m_flt41;
    stuReturn.m_flt24 = m_flt42;
    stuReturn.m_flt34 = m_flt43;
    stuReturn.m_flt44 = m_flt44;
    return stuReturn;
}