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