00001 #include <windows.h>
00002 #include "locmath.h"
00003 #include "vector.h"
00004 #include "quat.h"
00005 #include "vertex.h"
00006
00007 void LoadIdentity(float m[])
00008 {
00009 m[0]=1.0f;
00010 m[1]=0.0f;
00011 m[2]=0.0f;
00012 m[3]=0.0f;
00013
00014 m[4]=0.0f;
00015 m[5]=1.0f;
00016 m[6]=0.0f;
00017 m[7]=0.0f;
00018
00019 m[8]=0.0f;
00020 m[9]=0.0f;
00021 m[10]=1.0f;
00022 m[11]=0.0f;
00023
00024 m[12]=0.0f;
00025 m[13]=0.0f;
00026 m[14]=0.0f;
00027 m[15]=1.0f;
00028 }
00029
00030 void CopyMatrix(float m[], float n[])
00031 {
00032 m[0 ] = n[0 ];
00033 m[1 ] = n[1 ];
00034 m[2 ] = n[2 ];
00035 m[3 ] = n[3 ];
00036 m[4 ] = n[4 ];
00037 m[5 ] = n[5 ];
00038 m[6 ] = n[6 ];
00039 m[7 ] = n[7 ];
00040 m[8 ] = n[8 ];
00041 m[9 ] = n[9 ];
00042 m[10] = n[10];
00043 m[11] = n[11];
00044 m[12] = n[12];
00045 m[13] = n[13];
00046 m[14] = n[14];
00047 m[15] = n[15];
00048 }
00049
00050 void MultMatrix(float m[], float n[])
00051 {
00052 float temp[16];
00053
00054 CopyMatrix(temp, m);
00055 m[0] = temp[0 ]*n[0 ]
00056 + temp[4 ]*n[1 ]
00057 + temp[8 ]*n[2 ]
00058 + temp[12]*n[3 ];
00059
00060 m[1] = temp[1 ]*n[0 ]
00061 + temp[5 ]*n[1 ]
00062 + temp[9 ]*n[2 ]
00063 + temp[13]*n[3 ];
00064
00065 m[2] = temp[2 ]*n[0 ]
00066 + temp[6 ]*n[1 ]
00067 + temp[10]*n[2 ]
00068 + temp[14]*n[3 ];
00069
00070 m[3] = temp[3 ]*n[0 ]
00071 + temp[7 ]*n[1 ]
00072 + temp[11]*n[2 ]
00073 + temp[15]*n[3 ];
00074
00075 m[4] = temp[0 ]*n[4 ]
00076 + temp[4 ]*n[5 ]
00077 + temp[8 ]*n[6 ]
00078 + temp[12]*n[7 ];
00079
00080 m[5] = temp[1 ]*n[4 ]
00081 + temp[5 ]*n[5 ]
00082 + temp[9 ]*n[6 ]
00083 + temp[13]*n[7 ];
00084
00085 m[6] = temp[2 ]*n[4 ]
00086 + temp[6 ]*n[5 ]
00087 + temp[10]*n[6 ]
00088 + temp[14]*n[7 ];
00089
00090 m[7] = temp[3 ]*n[4 ]
00091 + temp[7 ]*n[5 ]
00092 + temp[11]*n[6 ]
00093 + temp[15]*n[7 ];
00094
00095 m[8] = temp[0 ]*n[8 ]
00096 + temp[4 ]*n[9 ]
00097 + temp[8 ]*n[10]
00098 + temp[12]*n[11];
00099
00100 m[9] = temp[1 ]*n[8 ]
00101 + temp[5 ]*n[9 ]
00102 + temp[9 ]*n[10]
00103 + temp[13]*n[11];
00104
00105 m[10]= temp[2 ]*n[8 ]
00106 + temp[6 ]*n[9 ]
00107 + temp[10]*n[10]
00108 + temp[14]*n[11];
00109
00110 m[11]= temp[3 ]*n[8 ]
00111 + temp[7 ]*n[9 ]
00112 + temp[11]*n[10]
00113 + temp[15]*n[11];
00114
00115 m[12]= temp[0 ]*n[12]
00116 + temp[4 ]*n[13]
00117 + temp[8 ]*n[14]
00118 + temp[12]*n[15];
00119
00120 m[13]= temp[1 ]*n[12]
00121 + temp[5 ]*n[13]
00122 + temp[9 ]*n[14]
00123 + temp[13]*n[15];
00124
00125 m[14]= temp[2 ]*n[12]
00126 + temp[6 ]*n[13]
00127 + temp[10]*n[14]
00128 + temp[14]*n[15];
00129
00130 m[15]= temp[3 ]*n[12]
00131 + temp[7 ]*n[13]
00132 + temp[11]*n[14]
00133 + temp[15]*n[15];
00134 }
00135
00136 void MatrixInverse(float m[])
00137 {
00138 float n[16];
00139
00140 CopyMatrix(n, m);
00141 m[0 ] = n[0 ];
00142 m[1 ] = n[4 ];
00143 m[2 ] = n[8 ];
00144
00145 m[4 ] = n[1 ];
00146 m[5 ] = n[5 ];
00147 m[6 ] = n[9 ];
00148
00149 m[8 ] = n[2 ];
00150 m[9 ] = n[6 ];
00151 m[10] = n[10];
00152
00153 m[12] *= -1.0f;
00154 m[13] *= -1.0f;
00155 m[14] *= -1.0f;
00156 }
00157
00158
00159
00160
00161
00162
00163
00164
00165 QUAT AxisAngleToMatrix(VECTOR axis, float theta, float m[16])
00166 {
00167 QUAT q;
00168 float halfTheta = theta * 0.5;
00169 float cosHalfTheta = cos(halfTheta);
00170 float sinHalfTheta = sin(halfTheta);
00171 float xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
00172 q.x = axis.x * sinHalfTheta;
00173 q.y = axis.y * sinHalfTheta;
00174 q.z = axis.z * sinHalfTheta;
00175 q.w = cosHalfTheta;
00176 xs = q.x * 2; ys = q.y * 2; zs = q.z * 2;
00177 wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
00178 xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
00179 yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
00180 m[0] = 1 - (yy + zz);
00181 m[1] = xy - wz;
00182 m[2] = xz + wy;
00183 m[4] = xy + wz;
00184 m[5] = 1 - (xx + zz);
00185 m[6] = yz - wx;
00186 m[8] = xz - wy;
00187 m[9] = yz + wx;
00188 m[10] = 1 - (xx + yy);
00189
00190 m[12] = m[13] = m[14] = m[3] = m[7] = m[11] = 0;
00191 m[15] = 1;
00192 return (q);
00193 }
00194
00195 float DotProduct(VECTOR vec1, VECTOR vec2)
00196 {
00197
00198
00199
00200
00201
00202
00203
00204
00205 float dot;
00206 dot = vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
00207 return dot;
00208 }
00209
00210 VECTOR CrossVector(VECTOR vec1, VECTOR vec2)
00211 {
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223 VECTOR vec3;
00224 vec3.x = vec1.y * vec2.z - vec1.z * vec2.y;
00225 vec3.y = vec1.z * vec2.x - vec1.x * vec2.z;
00226 vec3.z = vec1.x * vec2.y - vec1.y * vec2.x;
00227 return vec3;
00228 }
00229
00230 void EulerToQuat(float roll, float pitch, float yaw, QUAT * quat)
00231 {
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241 float cr, cp, cy, sr, sp, sy, cpcy, spsy;
00242 cr = cos(roll/2);
00243 cp = cos(pitch/2);
00244 cy = cos(yaw/2);
00245 sr = sin(roll/2);
00246 sp = sin(pitch/2);
00247 sy = sin(yaw/2);
00248 cpcy = cp * cy;
00249 spsy = sp * sy;
00250 quat->w = cr * cpcy + sr * spsy;
00251 quat->x = sr * cpcy - cr * spsy;
00252 quat->y = cr * sp * cy + sr * cp * sy;
00253 quat->z = cr * cp * sy - sr * sp * cy;
00254 }
00255
00256 float MagnitudeQuat(QUAT q1)
00257 {
00258 return( sqrt(q1.w*q1.w+q1.x*q1.x+q1.y*q1.y+q1.z*q1.z));
00259 }
00260
00261 QUAT NormaliseQuat(QUAT q1)
00262 {
00263 QUAT q2;
00264 float Mag;
00265 Mag = MagnitudeQuat(q1);
00266 q2.w = q1.w/Mag;
00267 q2.x = q1.x/Mag;
00268 q2.y = q1.y/Mag;
00269 q2.z = q1.z/Mag;
00270 return q2;
00271 }
00272
00273 void QuatToMatrix(QUAT quat, float m[16])
00274 {
00275 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
00276
00277 x2 = quat.x + quat.x;
00278 y2 = quat.y + quat.y;
00279 z2 = quat.z + quat.z;
00280 xx = quat.x * x2;
00281 xy = quat.x * y2;
00282 xz = quat.x * z2;
00283 yy = quat.y * y2;
00284 yz = quat.y * z2;
00285 zz = quat.z * z2;
00286 wx = quat.w * x2;
00287 wy = quat.w * y2;
00288 wz = quat.w * z2;
00289 m[0] = 1.0 - (yy + zz);
00290 m[1] = xy - wz;
00291 m[2] = xz + wy;
00292 m[3] = 0.0;
00293 m[4] = xy + wz;
00294 m[5] = 1.0 - (xx + zz);
00295 m[6] = yz - wx;
00296 m[7] = 0.0;
00297 m[8] = xz - wy;
00298 m[9] = yz + wx;
00299 m[10] = 1.0 - (xx + yy);
00300 m[11] = 0.0;
00301 m[12] = 0;
00302 m[13] = 0;
00303 m[14] = 0;
00304 m[15] = 1;
00305 }
00306
00307 QUAT MultQuat(QUAT q1, QUAT q2)
00308 {
00309
00310
00311
00312
00313
00314
00315 QUAT q3;
00316 VECTOR vectorq1;
00317 VECTOR vectorq2;
00318 vectorq1.x = q1.x;
00319 vectorq1.y = q1.y;
00320 vectorq1.z = q1.z;
00321 vectorq2.x = q2.x;
00322 vectorq2.y = q2.y;
00323 vectorq2.z = q2.z;
00324
00325 VECTOR tempvec1;
00326 VECTOR tempvec2;
00327 VECTOR tempvec3;
00328 q3.w = (q1.w*q2.w) - DotProduct(vectorq1, vectorq2);
00329 tempvec1 = CrossVector(vectorq1, vectorq2);
00330 tempvec2.x = q1.w * q2.x;
00331 tempvec2.y = q1.w * q2.y;
00332 tempvec2.z = q1.w * q2.z;
00333 tempvec3.x = q2.w * q1.x;
00334 tempvec3.y = q2.w * q1.y;
00335 tempvec3.z = q2.w * q1.z;
00336 q3.x = tempvec1.x + tempvec2.x + tempvec3.x;
00337 q3.y = tempvec1.y + tempvec2.y + tempvec3.y;
00338 q3.z = tempvec1.z + tempvec2.z + tempvec3.z;
00339 return NormaliseQuat(q3);
00340 }
00341
00342
00343 VECTOR GetNormal(VECTOR vertex1, VECTOR vertex2, VECTOR vertex3)
00344 {
00345 float ux, uy, uz, vx, vy, vz;
00346 VECTOR temp_vertex;
00347
00348 ux = vertex1.x - vertex2.x;
00349 uy = vertex1.y - vertex2.y;
00350 uz = vertex1.z - vertex2.z;
00351 vx = vertex3.x - vertex2.x;
00352 vy = vertex3.y - vertex2.y;
00353 vz = vertex3.z - vertex2.z;
00354 temp_vertex.x = (uy*vz)-(vy*uz);
00355 temp_vertex.y = (uz*vx)-(vz*ux);
00356 temp_vertex.z = (ux*vy)-(vx*uy);
00357 return temp_vertex;
00358 }
00359
00360 VERTEX GetNorm(float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3)
00361 {
00362 float ux;
00363 float uy;
00364 float uz;
00365 float vx;
00366 float vy;
00367 float vz;
00368 VERTEX temp_vertex;
00369 ux = x1 - x2;
00370 uy = y1 - y2;
00371 uz = z1 - z2;
00372 vx = x3 - x2;
00373 vy = y3 - y2;
00374 vz = z3 - z2;
00375 temp_vertex.nx = (uy*vz)-(vy*uz);
00376 temp_vertex.ny = (uz*vx)-(vz*ux);
00377 temp_vertex.nz = (ux*vy)-(vx*uy);
00378 return temp_vertex;
00379 }
00380
00381 GLfloat MagnitudeVector(VECTOR vec1)
00382 {
00383 return(sqrt(vec1.x*vec1.x+vec1.y*vec1.y+vec1.z*vec1.z));
00384 }
00385
00386 VECTOR GetUnitVector(VECTOR vector)
00387 {
00388
00389
00390
00391
00392 float length = (float) sqrt(( vector.x * vector.x) +
00393 ( vector.y * vector.y) +
00394 ( vector.z * vector.z) );
00395
00396
00397
00398 if(length == 0.0f)
00399 length = 1.0f;
00400
00401
00402
00403 vector.x /= length;
00404 vector.y /= length;
00405 vector.z /= length;
00406 return vector;
00407 }
00408
00409 VECTOR GetEdgeVector(VECTOR point1, VECTOR point2)
00410 {
00411 VECTOR temp_vector;
00412 temp_vector.x = point1.x - point2.x;
00413 temp_vector.y = point1.y - point2.y;
00414 temp_vector.z = point1.z - point2.z;
00415 return temp_vector;
00416 }