www.pudn.com > Gimcrack-v0.0051-Source.zip > matrix.cpp
#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");
}
}