matrix.cpp

小型的3D游戏引擎

#include "matrix.h"
#include "../global.h"

GcMatrix3::GcMatrix3()
{
	 SetIdentity();
}

float & GcMatrix3::operator[]( int i )
{
	 return array[i];
}

GcMatrix3 & GcMatrix3::operator=( const float * m )
{
	for(int i = 0; i < 9; i++)
        array[i] = m[i];

    return *this;  
}

GcMatrix3 & GcMatrix3::operator=( const GcMatrix4 & m )
{
	
	array[0] = m[0];
	array[1] = m[1];
	array[2] = m[2];
	array[3] = m[4];
	array[4] = m[5];
	array[5] = m[6];
	array[6] = m[8];
	array[7] = m[9];
	array[8] = m[10];
	

    return *this;  
}


GcMatrix3 & GcMatrix3::operator*=( const GcMatrix3 & m )
{
	GcMatrix3 tmp;

 	tmp.a00 = a00 * m.a00 + a01 * m.a10 + a02 * m.a20;
	tmp.a01 = a00 * m.a01 + a01 * m.a11 + a02 * m.a21;
	tmp.a02 = a00 * m.a02 + a01 * m.a12 + a02 * m.a22;
	
	tmp.a10 = a10 * m.a00 + a11 * m.a10 + a12 * m.a20;
	tmp.a11 = a10 * m.a01 + a11 * m.a11 + a12 * m.a21;
	tmp.a12 = a10 * m.a02 + a11 * m.a12 + a12 * m.a22;
	
	tmp.a20 = a20 * m.a00 + a21 * m.a10 + a22 * m.a20;
	tmp.a21 = a20 * m.a01 + a21 * m.a11 + a22 * m.a21;
	tmp.a22 = a20 * m.a02 + a21 * m.a12 + a22 * m.a22;
    
    for(int i = 0; i < 16; i++)
        array[i]  = tmp[i];

    return *this;
}

GcMatrix3 GcMatrix3::operator*( const GcMatrix3 & m ) const
{
    GcMatrix3 tmp;

	tmp.a00 = a00*m.a00 + a01*m.a10 + a02*m.a20;
	tmp.a01 = a00*m.a01 + a01*m.a11 + a02*m.a21;
	tmp.a02 = a00*m.a02 + a01*m.a12 + a02*m.a22;
	

	tmp.a10 = a10*m.a00 + a11*m.a10 + a12*m.a20;
	tmp.a11 = a10*m.a01 + a11*m.a11 + a12*m.a21;
	tmp.a12 = a10*m.a02 + a11*m.a12 + a12*m.a22;
	

	tmp.a20 = a20*m.a00 + a21*m.a10 + a22*m.a20;
	tmp.a21 = a20*m.a01 + a21*m.a11 + a22*m.a21;
	tmp.a22 = a20*m.a02 + a21*m.a12 + a22*m.a22;
	

	
	return tmp;
}

GcVector3 GcMatrix3::operator*( const GcVector3 & v ) const
{
	GcVector3 result;

	result[0] = array[0] * v.array[0] + array[3] * v.array[1] + array[6] * v.array[2];
	result[1] = array[1] * v.array[0] + array[4] * v.array[1] + array[7] * v.array[2];
	result[2] = array[2] * v.array[0] + array[5] * v.array[1] + array[8] * v.array[2];
	
	return result;
}



GcMatrix3 GcMatrix3::operator+( const GcMatrix3 & m )
{
	GcMatrix3 result;

	for( int i = 0; i < 9; ++i )
		result[i] = array[i] + m.array[i];
	
	return result;
}

GcMatrix3 GcMatrix3::operator-( const GcMatrix3 & m )
{
	GcMatrix3 result;

	for( int i = 0; i < 9; ++i )
		result[i] = array[i] - m.array[i];
	
	return result;
}

GcMatrix3::operator const float*() const
{
    return array;
}

GcMatrix3::operator float*()
{
    return array;
}

GcMatrix3 & GcMatrix3::BuildFromEuler( const GcVector3 & angles )
{
	
	float A = (float)cos(angles[0] / RADIANS);
    float B = (float)sin(angles[0] / RADIANS);
    float C = (float)cos(angles[1] / RADIANS);
    float D = (float)sin(angles[1] / RADIANS);
    float E = (float)cos(angles[2] / RADIANS);
    float F = (float)sin(angles[2] / RADIANS);

    float AD = A * D;
    float BD = B * D;

    a00 = C * E;
    a01 = -C * F;
    a02 = D;

    a10 = BD * E + A * F;
    a11 = -BD * F + A * E;
    a12 = -B * C;

    a20 = -AD * E + B * F;
    a21 = AD * F + B * E;
    a22 = A * C;


    return *this;
}


int GcMatrix3::ConvertToEuler( GcVector3 & angles ) const
{
	
    float C;
    int retValue = 0;
    
	angles[1] = (float)(asin(a02));
    C = (float)(cos(angles[1]));
    
    angles[1] *= (float)RADIANS;

    if(fabs(C) > 0.005)  // Gimbal lock?
    {
        angles[0] = (float)(atan2(-a12, a22) * RADIANS); // X axis

        angles[2] = (float)(atan2(-a01, a00) * RADIANS); // Z axis
    }
    else
    {
        retValue = -1;

        angles[0] = 0;

        angles[2] = (float)(atan2(a10, a11) * RADIANS);
    }
    
    angles[0] = NormalizeAngle(angles[0]);
    angles[1] = NormalizeAngle(angles[1]);
    angles[2] = NormalizeAngle(angles[2]);
	
    return retValue;
}


// Transpose the matrix ("Flip" the matrix around the identity)
GcMatrix3 & GcMatrix3::Transpose()
{
    Swap(a01, a10);
    Swap(a02, a20);
    Swap(a12, a21);
    
    return *this;
}



// Set the identity matrix
GcMatrix3 & GcMatrix3::SetIdentity()
{
    // Set the major diagonal to 1
	a00 = a11 = a22 = 1;
    
	// And the rest to 0
	a01 = a02 = 
	a10 = a12 =
	a20 = a21 = 0;
    
	return *this;
}


GcVector3 GcMatrix3::Axis(int i) const
{
	switch(i)
	{
		case 0: return XAxis();
		case 1: return YAxis();
		case 2: return ZAxis();
		default: return GcVector3(0,0,0); g_Debug->Log("No vector returned in GcMatrix4::Axis\n");
	}
}



/////////////////////////////////////7




GcMatrix4::GcMatrix4()
{
	 SetIdentity();
}

float & GcMatrix4::operator[](int i)
{
	 return array[i];
}

GcMatrix4 & GcMatrix4::operator=(const float * m )
{
	for(int i = 0; i < 16; i++)
        array[i] = m[i];

    return *this;  
}

GcMatrix4 & GcMatrix4::operator*=( const GcMatrix4 & m )
{
	GcMatrix4 tmp;

    tmp.a00 = a00*m.a00 + a01*m.a10 + a02*m.a20 + a03*m.a30;
    tmp.a01 = a00*m.a01 + a01*m.a11 + a02*m.a21 + a03*m.a31;
    tmp.a02 = a00*m.a02 + a01*m.a12 + a02*m.a22 + a03*m.a32;
    tmp.a03 = a00*m.a03 + a01*m.a13 + a02*m.a23 + a03*m.a33;

    tmp.a10 = a10*m.a00 + a11*m.a10 + a12*m.a20 + a13*m.a30;
    tmp.a11 = a10*m.a01 + a11*m.a11 + a12*m.a21 + a13*m.a31;
    tmp.a12 = a10*m.a02 + a11*m.a12 + a12*m.a22 + a13*m.a32;
    tmp.a13 = a10*m.a03 + a11*m.a13 + a12*m.a23 + a13*m.a33;

    tmp.a20 = a20*m.a00 + a21*m.a10 + a22*m.a20 + a23*m.a30;
    tmp.a21 = a20*m.a01 + a21*m.a11 + a22*m.a21 + a23*m.a31;
    tmp.a22 = a20*m.a02 + a21*m.a12 + a22*m.a22 + a23*m.a32;
    tmp.a23 = a20*m.a03 + a21*m.a13 + a22*m.a23 + a23*m.a33;

    tmp.a30 = a30*m.a00 + a31*m.a10 + a32*m.a20 + a33*m.a30;
    tmp.a31 = a30*m.a01 + a31*m.a11 + a32*m.a21 + a33*m.a31;
    tmp.a32 = a30*m.a02 + a31*m.a12 + a32*m.a22 + a33*m.a32;
    tmp.a33 = a30*m.a03 + a31*m.a13 + a32*m.a23 + a33*m.a33;
    
    for(int i = 0; i < 16; i++)
        array[i]  = tmp[i];

    return *this;
}

GcMatrix4 GcMatrix4::operator*( const GcMatrix4 & m ) const
{
    GcMatrix4 tmp;

	tmp.a00 = a00*m.a00 + a01*m.a10 + a02*m.a20 + a03*m.a30;
	tmp.a01 = a00*m.a01 + a01*m.a11 + a02*m.a21 + a03*m.a31;
	tmp.a02 = a00*m.a02 + a01*m.a12 + a02*m.a22 + a03*m.a32;
	tmp.a03 = a00*m.a03 + a01*m.a13 + a02*m.a23 + a03*m.a33;

	tmp.a10 = a10*m.a00 + a11*m.a10 + a12*m.a20 + a13*m.a30;
	tmp.a11 = a10*m.a01 + a11*m.a11 + a12*m.a21 + a13*m.a31;
	tmp.a12 = a10*m.a02 + a11*m.a12 + a12*m.a22 + a13*m.a32;
	tmp.a13 = a10*m.a03 + a11*m.a13 + a12*m.a23 + a13*m.a33;

	tmp.a20 = a20*m.a00 + a21*m.a10 + a22*m.a20 + a23*m.a30;
	tmp.a21 = a20*m.a01 + a21*m.a11 + a22*m.a21 + a23*m.a31;
	tmp.a22 = a20*m.a02 + a21*m.a12 + a22*m.a22 + a23*m.a32;
	tmp.a23 = a20*m.a03 + a21*m.a13 + a22*m.a23 + a23*m.a33;

	tmp.a30 = a30*m.a00 + a31*m.a10 + a32*m.a20 + a33*m.a30;
	tmp.a31 = a30*m.a01 + a31*m.a11 + a32*m.a21 + a33*m.a31;
	tmp.a32 = a30*m.a02 + a31*m.a12 + a32*m.a22 + a33*m.a32;
	tmp.a33 = a30*m.a03 + a31*m.a13 + a32*m.a23 + a33*m.a33;

	return tmp;
}

GcVector3 GcMatrix4::operator*( const GcVector3 & v ) const
{
	GcVector3 result;

	result[0] = array[0] * v.array[0] + array[4] * v.array[1] + array[8]  * v.array[2] + array[12];
	result[1] = array[1] * v.array[0] + array[5] * v.array[1] + array[9]  * v.array[2] + array[13];
	result[2] = array[2] * v.array[0] + array[6] * v.array[1] + array[10] * v.array[2] + array[14];
	
	return result;
}

GcVector4 GcMatrix4::operator*( const GcVector4 & v ) const
{
	GcVector4 result;

	result.array[0] = array[0] * v.array[0] + array[4] * v.array[1] + array[8]  * v.array[2] + array[12] * v.array[3];
	result.array[1] = array[1] * v.array[0] + array[5] * v.array[1] + array[9]  * v.array[2] + array[13] * v.array[3];
	result.array[2] = array[2] * v.array[0] + array[6] * v.array[1] + array[10] * v.array[2] + array[14] * v.array[3];
	result.array[3] = array[3] * v.array[0] + array[7] * v.array[1] + array[11] * v.array[2] + array[15] * v.array[3];
  
	result.Homogenize();
	
	return result;
}


GcMatrix4 GcMatrix4::operator+( const GcMatrix4 & m )
{
	GcMatrix4 result;

	for( int i = 0; i < 16; ++i )
		result[i] = array[i] + m.array[i];
	
	return result;
}

GcMatrix4 GcMatrix4::operator-( const GcMatrix4 & m )
{
	GcMatrix4 result;

	for( int i = 0; i < 16; ++i )
		result[i] = array[i] - m.array[i];
	
	return result;
}

GcMatrix4::operator const float*() const
{
    return array;
}

GcMatrix4::operator float*()
{
    return array;
}

GcMatrix4 & GcMatrix4::BuildFromEuler( const GcVector3 & angles )
{
	
	float A = (float)cos(angles[0] / RADIANS);
    float B = (float)sin(angles[0] / RADIANS);
    float C = (float)cos(angles[1] / RADIANS);
    float D = (float)sin(angles[1] / RADIANS);
    float E = (float)cos(angles[2] / RADIANS);
    float F = (float)sin(angles[2] / RADIANS);

    float AD = A * D;
    float BD = B * D;

    a00 = C * E;
    a01 = -C * F;
    a02 = D;

    a10 = BD * E + A * F;
    a11 = -BD * F + A * E;
    a12 = -B * C;

    a20 = -AD * E + B * F;
    a21 = AD * F + B * E;
    a22 = A * C;

    a30 = a31 = a32 = a03 = a13 = a23 = 0;
    a33 = 1;

    return *this;
}

GcMatrix4 & GcMatrix4::BuildFromEuler( const GcVector4 & angles )
{
	
	float A = (float)cos(angles[0] / RADIANS);
    float B = (float)sin(angles[0] / RADIANS);
    float C = (float)cos(angles[1] / RADIANS);
    float D = (float)sin(angles[1] / RADIANS);
    float E = (float)cos(angles[2] / RADIANS);
    float F = (float)sin(angles[2] / RADIANS);

    float AD = A * D;
    float BD = B * D;

    a00 = C * E;
    a01 = -C * F;
    a02 = D;

    a10 = BD * E + A * F;
    a11 = -BD * F + A * E;
    a12 = -B * C;

    a20 = -AD * E + B * F;
    a21 = AD * F + B * E;
    a22 = A * C;

    a30 = a31 = a32 = a03 = a13 = a23 = 0;
    a33 = 1;

    return *this;
}

int GcMatrix4::ConvertToEuler( GcVector3 & angles ) const
{
	
    float C;
    int retValue = 0;
    
	angles[1] = (float)(asin(a02));
    C = (float)(cos(angles[1]));
    
    angles[1] *= (float)RADIANS;

    if(fabs(C) > 0.005)  // Gimbal lock?
    {
        angles[0] = (float)(atan2(-a12, a22) * RADIANS); // X axis

        angles[2] = (float)(atan2(-a01, a00) * RADIANS); // Z axis
    }
    else
    {
        retValue = -1;

        angles[0] = 0;

        angles[2] = (float)(atan2(a10, a11) * RADIANS);
    }
    
    angles[0] = NormalizeAngle(angles[0]);
    angles[1] = NormalizeAngle(angles[1]);
    angles[2] = NormalizeAngle(angles[2]);
	
    return retValue;
}

int GcMatrix4::ConvertToEuler( GcVector4 & angles ) const
{
	
    float C;
    int retValue = 0;
    
	angles[1] = (float)(asin(a02));
    C = (float)(cos(angles[1]));
    
    angles[1] *= (float)RADIANS;

    if(fabs(C) > 0.005)  // gimbal lock?
    {
        angles[0] = (float)(atan2(-a12, a22)*RADIANS); // X axis

        angles[2] = (float)(atan2(-a01, a00)*RADIANS); // Z axis
    }
    else
    {
        retValue = -1;

        angles[0] = 0;

        angles[2] = (float)(atan2(a10, a11)*RADIANS);
    }
    angles[3] = 1;
    angles[0] = NormalizeAngle(angles[0]);
    angles[1] = NormalizeAngle(angles[1]);
    angles[2] = NormalizeAngle(angles[2]);
	
    return retValue;
}

// Transpose the matrix ("Flip" the matrix around the identity)
GcMatrix4 & GcMatrix4::Transpose()
{
    Swap(a01, a10);
    Swap(a02, a20);
    Swap(a03, a30);

    Swap(a12, a21);
    Swap(a13, a31);
    Swap(a23, a32);

    return *this;
}



// Set the identity matrix
GcMatrix4 & GcMatrix4::SetIdentity()
{
    // Set the major diagonal to 1
	a00 = a11 = a22 = a33 = 1;
    
	// And the rest to 0
	a01 = a02 = a03 = a10 = 
    a12 = a13 = a20 = a21 = 
    a23 = a30 = a31 = a32 = 0;
    
	return *this;
}

// Translate by the passed vector from the current translation
GcMatrix4 & GcMatrix4::Translate( const GcVector3 & v )
{
	// Position 12-14 of the matrix contains the translation (the "position")
	array[12] += v.x;
	array[13] += v.y;
	array[14] += v.z;

	return *this;
}

// Translate by the passed vector from the current translation
GcMatrix4 & GcMatrix4::Translate( const GcVector4 & v )
{
	// Position 12-14 of the matrix contains the translation (the "position")
	array[12] += v.x;
	array[13] += v.y;
	array[14] += v.z;

	return *this;
}

// Set the translation part of the matrix to specified vector 
Matrix4 & GcMatrix4::SetTranslation(const GcVector3 & v)
{
    
	array[12] = v.x;
    array[13] = v.y;
    array[14] = v.z;

    return *this;
}

// Set the translation part of the matrix to specified vector 
Matrix4 & GcMatrix4::SetTranslation(const GcVector4 & v)
{
    
	array[12] = v.x;
    array[13] = v.y;
    array[14] = v.z;

    return *this;
}

GcVector3 GcMatrix4::Axis( int i) const
{
	switch( i )
	{
		case 0: return XAxis();
		case 1: return YAxis();
		case 2: return ZAxis();
		default: return GcVector3(0,0,0); g_Debug->Log("No vector returned in GcMatrix4::Axis\n");
	}
}