00001 #include <windows.h>
00002 #include "matrix.h"
00003
00004
00005 MATRIX::MATRIX()
00006 {
00007 LoadIdentity();
00008 }
00009
00010 MATRIX::~MATRIX()
00011 {
00012 }
00013
00014 void MATRIX::LoadIdentity()
00015 {
00016 Element[0]=1.0f;
00017 Element[1]=0.0f;
00018 Element[2]=0.0f;
00019 Element[3]=0.0f;
00020
00021 Element[4]=0.0f;
00022 Element[5]=1.0f;
00023 Element[6]=0.0f;
00024 Element[7]=0.0f;
00025
00026 Element[8]=0.0f;
00027 Element[9]=0.0f;
00028 Element[10]=1.0f;
00029 Element[11]=0.0f;
00030
00031 Element[12]=0.0f;
00032 Element[13]=0.0f;
00033 Element[14]=0.0f;
00034 Element[15]=1.0f;
00035 }
00036
00037 void MATRIX::CopyMatrix(float m[16])
00038 {
00039 Element[0 ] = m[0 ];
00040 Element[1 ] = m[1 ];
00041 Element[2 ] = m[2 ];
00042 Element[3 ] = m[3 ];
00043 Element[4 ] = m[4 ];
00044 Element[5 ] = m[5 ];
00045 Element[6 ] = m[6 ];
00046 Element[7 ] = m[7 ];
00047 Element[8 ] = m[8 ];
00048 Element[9 ] = m[9 ];
00049 Element[10] = m[10];
00050 Element[11] = m[11];
00051 Element[12] = m[12];
00052 Element[13] = m[13];
00053 Element[14] = m[14];
00054 Element[15] = m[15];
00055 }
00056
00057 void MATRIX::MultMatrix(float m[])
00058 {
00059 MATRIX temp;
00060
00061 temp.CopyMatrix(this->Element);
00062
00063 Element[0] = temp.Element[0 ]*m[0 ]
00064 + temp.Element[4 ]*m[1 ]
00065 + temp.Element[8 ]*m[2 ]
00066 + temp.Element[12]*m[3 ];
00067
00068 Element[1] = temp.Element[1 ]*m[0 ]
00069 + temp.Element[5 ]*m[1 ]
00070 + temp.Element[9 ]*m[2 ]
00071 + temp.Element[13]*m[3 ];
00072
00073 Element[2] = temp.Element[2 ]*m[0 ]
00074 + temp.Element[6 ]*m[1 ]
00075 + temp.Element[10]*m[2 ]
00076 + temp.Element[14]*m[3 ];
00077
00078 Element[3] = temp.Element[3 ]*m[0 ]
00079 + temp.Element[7 ]*m[1 ]
00080 + temp.Element[11]*m[2 ]
00081 + temp.Element[15]*m[3 ];
00082
00083 Element[4] = temp.Element[0 ]*m[4 ]
00084 + temp.Element[4 ]*m[5 ]
00085 + temp.Element[8 ]*m[6 ]
00086 + temp.Element[12]*m[7 ];
00087
00088 Element[5] = temp.Element[1 ]*m[4 ]
00089 + temp.Element[5 ]*m[5 ]
00090 + temp.Element[9 ]*m[6 ]
00091 + temp.Element[13]*m[7 ];
00092
00093 Element[6] = temp.Element[2 ]*m[4 ]
00094 + temp.Element[6 ]*m[5 ]
00095 + temp.Element[10]*m[6 ]
00096 + temp.Element[14]*m[7 ];
00097
00098 Element[7] = temp.Element[3 ]*m[4 ]
00099 + temp.Element[7 ]*m[5 ]
00100 + temp.Element[11]*m[6 ]
00101 + temp.Element[15]*m[7 ];
00102
00103 Element[8] = temp.Element[0 ]*m[8 ]
00104 + temp.Element[4 ]*m[9 ]
00105 + temp.Element[8 ]*m[10]
00106 + temp.Element[12]*m[11];
00107
00108 Element[9] = temp.Element[1 ]*m[8 ]
00109 + temp.Element[5 ]*m[9 ]
00110 + temp.Element[9 ]*m[10]
00111 + temp.Element[13]*m[11];
00112
00113 Element[10]= temp.Element[2 ]*m[8 ]
00114 + temp.Element[6 ]*m[9 ]
00115 + temp.Element[10]*m[10]
00116 + temp.Element[14]*m[11];
00117
00118 Element[11]= temp.Element[3 ]*m[8 ]
00119 + temp.Element[7 ]*m[9 ]
00120 + temp.Element[11]*m[10]
00121 + temp.Element[15]*m[11];
00122
00123 Element[12]= temp.Element[0 ]*m[12]
00124 + temp.Element[4 ]*m[13]
00125 + temp.Element[8 ]*m[14]
00126 + temp.Element[12]*m[15];
00127
00128 Element[13]= temp.Element[1 ]*m[12]
00129 + temp.Element[5 ]*m[13]
00130 + temp.Element[9 ]*m[14]
00131 + temp.Element[13]*m[15];
00132
00133 Element[14]= temp.Element[2 ]*m[12]
00134 + temp.Element[6 ]*m[13]
00135 + temp.Element[10]*m[14]
00136 + temp.Element[14]*m[15];
00137
00138 Element[15]= temp.Element[3 ]*m[12]
00139 + temp.Element[7 ]*m[13]
00140 + temp.Element[11]*m[14]
00141 + temp.Element[15]*m[15];
00142 }
00143
00144 void MATRIX::MatrixInverse()
00145 {
00146 MATRIX temp;
00147
00148 temp.CopyMatrix(this->Element);
00149
00150 Element[0 ] = temp.Element[0 ];
00151 Element[1 ] = temp.Element[4 ];
00152 Element[2 ] = temp.Element[8 ];
00153
00154 Element[4 ] = temp.Element[1 ];
00155 Element[5 ] = temp.Element[5 ];
00156 Element[6 ] = temp.Element[9 ];
00157
00158 Element[8 ] = temp.Element[2 ];
00159 Element[9 ] = temp.Element[6 ];
00160 Element[10] = temp.Element[10];
00161
00162 Element[12] *= -1.0f;
00163 Element[13] *= -1.0f;
00164 Element[14] *= -1.0f;
00165 }
00166
00167 void MATRIX::MatrixFromAxisAngle(VECTOR axis, float theta)
00168 {
00169 QUAT q;
00170 float halfTheta = theta * 0.5;
00171 float cosHalfTheta = cos(halfTheta);
00172 float sinHalfTheta = sin(halfTheta);
00173 float xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00174 q.x = axis.x * sinHalfTheta;
00175 q.y = axis.y * sinHalfTheta;
00176 q.z = axis.z * sinHalfTheta;
00177 q.w = cosHalfTheta;
00178 xs = q.x * 2; ys = q.y * 2; zs = q.z * 2;
00179 wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
00180 xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
00181 yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
00182 Element[0] = 1 - (yy + zz);
00183 Element[1] = xy - wz;
00184 Element[2] = xz + wy;
00185 Element[4] = xy + wz;
00186 Element[5] = 1 - (xx + zz);
00187 Element[6] = yz - wx;
00188 Element[8] = xz - wy;
00189 Element[9] = yz + wx;
00190 Element[10] = 1 - (xx + yy);
00191 Element[12] = Element[13] = Element[14] = Element[3] = Element[7] = Element[11] = 0;
00192 Element[15] = 1;
00193 }
00194
00195 void MATRIX::QuatToMatrix(QUAT quat)
00196 {
00197 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
00198
00199 x2 = quat.x + quat.x;
00200 y2 = quat.y + quat.y;
00201 z2 = quat.z + quat.z;
00202 xx = quat.x * x2;
00203 xy = quat.x * y2;
00204 xz = quat.x * z2;
00205 yy = quat.y * y2;
00206 yz = quat.y * z2;
00207 zz = quat.z * z2;
00208 wx = quat.w * x2;
00209 wy = quat.w * y2;
00210 wz = quat.w * z2;
00211 Element[0] = 1.0 - (yy + zz);
00212 Element[1] = xy - wz;
00213 Element[2] = xz + wy;
00214 Element[3] = 0.0;
00215 Element[4] = xy + wz;
00216 Element[5] = 1.0 - (xx + zz);
00217 Element[6] = yz - wx;
00218 Element[7] = 0.0;
00219 Element[8] = xz - wy;
00220 Element[9] = yz + wx;
00221 Element[10] = 1.0 - (xx + yy);
00222 Element[11] = 0.0;
00223 Element[12] = 0;
00224 Element[13] = 0;
00225 Element[14] = 0;
00226 Element[15] = 1;
00227 }