00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00029 #include <memory.h>
00030 #include <assert.h>
00031 #include <stdlib.h>
00032
00033 #include "utility.h"
00034 #include "vec3.h"
00035 #include "mat4.h"
00036 #include "quaternion.h"
00037
00045 kmMat4* kmMat4Fill(kmMat4* pOut, const kmScalar* pMat)
00046 {
00047 memcpy(pOut->mat, pMat, sizeof(kmScalar) * 16);
00048 return pOut;
00049 }
00050
00056 kmMat4* kmMat4Identity(kmMat4* pOut)
00057 {
00058 memset(pOut->mat, 0, sizeof(float) * 16);
00059 pOut->mat[0] = pOut->mat[5] = pOut->mat[10] = pOut->mat[15] = 1.0f;
00060 return pOut;
00061 }
00062
00068 kmMat4* kmMat4Inverse(kmMat4* pOut, const kmMat4* pM)
00069 {
00070 float temp[4];
00071 float mat[16];
00072 int i, j, k;
00073
00074 for (i = 0; i < 16; i++)
00075 {
00076 mat[i] = pM->mat[i];
00077 }
00078
00079 kmMat4Identity(pOut);
00080
00081 for (j = 0; j < 4; ++j)
00082 {
00083 int i1 = j;
00084
00085 for (i = j + 1; i < 4; ++i)
00086 {
00087 if (fabs(mat[i*4 + j]) > fabs(mat[i1*4 + j])) {
00088 i1 = i;
00089 }
00090 }
00091
00092
00093 for(k = 0; k < 4; k++)
00094 {
00095 temp[k] = mat[i1 * 4 + k];
00096 }
00097
00098 for(k = 0; k < 4; k++)
00099 {
00100 mat[i1 * 4 + k] = mat[j * 4 + k];
00101 mat[j * 4 + k] = temp[k];
00102 }
00103
00104 for(k = 0; k < 4; k++)
00105 {
00106 temp[k] = pOut->mat[i1 * 4 + k];
00107 }
00108
00109 for(k = 0; k < 4; k++)
00110 {
00111 pOut->mat[i1 * 4 + k] = pOut->mat[j * 4 + k];
00112 pOut->mat[j * 4 + k] = temp[k];
00113 }
00114
00115
00116 if (!mat[j*4 + j])
00117 {
00118
00119 return NULL;
00120 }
00121
00122 for(k = 0; k < 4; k++)
00123 {
00124 pOut->mat[j * 4 + k] /= mat[j * 4 + j];
00125 mat[j * 4 + k] /= mat[j * 4 + j];
00126 }
00127
00128
00129 for (i = 0; i < 4; ++i)
00130 {
00131 if (i != j)
00132 {
00133 for(k = 0; k < 4; k++)
00134 {
00135 pOut->mat[i*4 + k] -= mat[i*4 + j] * pOut->mat[j*4 + k];
00136 mat[i*4 + k] -= mat[i*4 + j] * mat[j*4 + k];
00137 }
00138 }
00139 }
00140 }
00141
00142 return pOut;
00143 }
00148 int kmMat4IsIdentity(const kmMat4* pIn)
00149 {
00150 static const float identity [] = { 1.0f, 0.0f, 0.0f, 0.0f,
00151 0.0f, 1.0f, 0.0f, 0.0f,
00152 0.0f, 0.0f, 1.0f, 0.0f,
00153 0.0f, 0.0f, 0.0f, 1.0f
00154 };
00155
00156 return (memcmp(identity, pIn->mat, sizeof(float) * 16) == 0);
00157 }
00158
00162 kmMat4* kmMat4Transpose(kmMat4* pOut, const kmMat4* pIn)
00163 {
00164 int x, z;
00165
00166 for (z = 0; z < 4; ++z) {
00167 for (x = 0; x < 4; ++x) {
00168 pOut->mat[(z * 4) + x] = pIn->mat[(x * 4) + z];
00169 }
00170 }
00171
00172 return pOut;
00173 }
00174
00178 kmMat4* kmMat4Multiply(kmMat4* pOut, const kmMat4* pM1, const kmMat4* pM2)
00179 {
00180 float mat[16];
00181
00182 const float *m1 = pM1->mat, *m2 = pM2->mat;
00183
00184 mat[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3];
00185 mat[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3];
00186 mat[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3];
00187 mat[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3];
00188
00189 mat[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7];
00190 mat[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7];
00191 mat[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7];
00192 mat[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7];
00193
00194 mat[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11];
00195 mat[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11];
00196 mat[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11];
00197 mat[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11];
00198
00199 mat[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15];
00200 mat[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15];
00201 mat[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
00202 mat[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
00203
00204
00205 memcpy(pOut->mat, mat, sizeof(float)*16);
00206
00207 return pOut;
00208 }
00209
00213 kmMat4* kmMat4Assign(kmMat4* pOut, const kmMat4* pIn)
00214 {
00215 assert(pOut != pIn && "You have tried to self-assign!!");
00216
00217 memcpy(pOut->mat, pIn->mat, sizeof(float)*16);
00218
00219 return pOut;
00220 }
00221
00225 int kmMat4AreEqual(const kmMat4* pMat1, const kmMat4* pMat2)
00226 {
00227 int i = 0;
00228
00229 assert(pMat1 != pMat2 && "You are comparing the same thing!");
00230
00231 for (i = 0; i < 16; ++i)
00232 {
00233 if (!(pMat1->mat[i] + kmEpsilon > pMat2->mat[i] &&
00234 pMat1->mat[i] - kmEpsilon < pMat2->mat[i])) {
00235 return KM_FALSE;
00236 }
00237 }
00238
00239 return KM_TRUE;
00240 }
00241
00246 kmMat4* kmMat4RotationAxis(kmMat4* pOut, const kmVec3* axis, kmScalar radians)
00247 {
00248 float rcos = cosf(radians);
00249 float rsin = sinf(radians);
00250
00251 pOut->mat[0] = rcos + axis->x * axis->x * (1 - rcos);
00252 pOut->mat[1] = axis->z * rsin + axis->y * axis->x * (1 - rcos);
00253 pOut->mat[2] = -axis->y * rsin + axis->z * axis->x * (1 - rcos);
00254 pOut->mat[3] = 0.0f;
00255
00256 pOut->mat[4] = -axis->z * rsin + axis->x * axis->y * (1 - rcos);
00257 pOut->mat[5] = rcos + axis->y * axis->y * (1 - rcos);
00258 pOut->mat[6] = axis->x * rsin + axis->z * axis->y * (1 - rcos);
00259 pOut->mat[7] = 0.0f;
00260
00261 pOut->mat[8] = axis->y * rsin + axis->x * axis->z * (1 - rcos);
00262 pOut->mat[9] = -axis->x * rsin + axis->y * axis->z * (1 - rcos);
00263 pOut->mat[10] = rcos + axis->z * axis->z * (1 - rcos);
00264 pOut->mat[11] = 0.0f;
00265
00266 pOut->mat[12] = 0.0f;
00267 pOut->mat[13] = 0.0f;
00268 pOut->mat[14] = 0.0f;
00269 pOut->mat[15] = 1.0f;
00270
00271 return pOut;
00272 }
00273
00277 kmMat4* kmMat4RotationX(kmMat4* pOut, const float radians)
00278 {
00279
00280
00281
00282
00283
00284
00285
00286
00287 pOut->mat[0] = 1.0f;
00288 pOut->mat[1] = 0.0f;
00289 pOut->mat[2] = 0.0f;
00290 pOut->mat[3] = 0.0f;
00291
00292 pOut->mat[4] = 0.0f;
00293 pOut->mat[5] = cosf(radians);
00294 pOut->mat[6] = sinf(radians);
00295 pOut->mat[7] = 0.0f;
00296
00297 pOut->mat[8] = 0.0f;
00298 pOut->mat[9] = -sinf(radians);
00299 pOut->mat[10] = cosf(radians);
00300 pOut->mat[11] = 0.0f;
00301
00302 pOut->mat[12] = 0.0f;
00303 pOut->mat[13] = 0.0f;
00304 pOut->mat[14] = 0.0f;
00305 pOut->mat[15] = 1.0f;
00306
00307 return pOut;
00308 }
00309
00314 kmMat4* kmMat4RotationY(kmMat4* pOut, const float radians)
00315 {
00316
00317
00318
00319
00320
00321
00322
00323 pOut->mat[0] = cosf(radians);
00324 pOut->mat[1] = 0.0f;
00325 pOut->mat[2] = -sinf(radians);
00326 pOut->mat[3] = 0.0f;
00327
00328 pOut->mat[4] = 0.0f;
00329 pOut->mat[5] = 1.0f;
00330 pOut->mat[6] = 0.0f;
00331 pOut->mat[7] = 0.0f;
00332
00333 pOut->mat[8] = sinf(radians);
00334 pOut->mat[9] = 0.0f;
00335 pOut->mat[10] = cosf(radians);
00336 pOut->mat[11] = 0.0f;
00337
00338 pOut->mat[12] = 0.0f;
00339 pOut->mat[13] = 0.0f;
00340 pOut->mat[14] = 0.0f;
00341 pOut->mat[15] = 1.0f;
00342
00343 return pOut;
00344 }
00345
00350 kmMat4* kmMat4RotationZ(kmMat4* pOut, const float radians)
00351 {
00352
00353
00354
00355
00356
00357
00358
00359 pOut->mat[0] = cosf(radians);
00360 pOut->mat[1] = sinf(radians);
00361 pOut->mat[2] = 0.0f;
00362 pOut->mat[3] = 0.0f;
00363
00364 pOut->mat[4] = -sinf(radians);
00365 pOut->mat[5] = cosf(radians);
00366 pOut->mat[6] = 0.0f;
00367 pOut->mat[7] = 0.0f;
00368
00369 pOut->mat[8] = 0.0f;
00370 pOut->mat[9] = 0.0f;
00371 pOut->mat[10] = 1.0f;
00372 pOut->mat[11] = 0.0f;
00373
00374 pOut->mat[12] = 0.0f;
00375 pOut->mat[13] = 0.0f;
00376 pOut->mat[14] = 0.0f;
00377 pOut->mat[15] = 1.0f;
00378
00379 return pOut;
00380 }
00381
00386 kmMat4* kmMat4RotationPitchYawRoll(kmMat4* pOut, const kmScalar pitch, const kmScalar yaw, const kmScalar roll)
00387 {
00388 double cr = cos(pitch);
00389 double sr = sin(pitch);
00390 double cp = cos(yaw);
00391 double sp = sin(yaw);
00392 double cy = cos(roll);
00393 double sy = sin(roll);
00394 double srsp = sr * sp;
00395 double crsp = cr * sp;
00396
00397 pOut->mat[0] = (kmScalar) cp * cy;
00398 pOut->mat[4] = (kmScalar) cp * sy;
00399 pOut->mat[8] = (kmScalar) - sp;
00400
00401 pOut->mat[1] = (kmScalar) srsp * cy - cr * sy;
00402 pOut->mat[5] = (kmScalar) srsp * sy + cr * cy;
00403 pOut->mat[9] = (kmScalar) sr * cp;
00404
00405 pOut->mat[2] = (kmScalar) crsp * cy + sr * sy;
00406 pOut->mat[6] = (kmScalar) crsp * sy - sr * cy;
00407 pOut->mat[10] = (kmScalar) cr * cp;
00408
00409 pOut->mat[3] = pOut->mat[7] = pOut->mat[11] = 0.0;
00410 pOut->mat[15] = 1.0;
00411
00412 return pOut;
00413 }
00414
00418 kmMat4* kmMat4RotationQuaternion(kmMat4* pOut, const kmQuaternion* pQ)
00419 {
00420 pOut->mat[ 0] = 1.0f - 2.0f * (pQ->y * pQ->y + pQ->z * pQ->z );
00421 pOut->mat[ 4] = 2.0f * (pQ->x * pQ->y + pQ->z * pQ->w);
00422 pOut->mat[ 8] = 2.0f * (pQ->x * pQ->z - pQ->y * pQ->w);
00423 pOut->mat[12] = 0.0f;
00424
00425
00426 pOut->mat[ 1] = 2.0f * ( pQ->x * pQ->y - pQ->z * pQ->w );
00427 pOut->mat[ 5] = 1.0f - 2.0f * ( pQ->x * pQ->x + pQ->z * pQ->z );
00428 pOut->mat[ 9] = 2.0f * (pQ->z * pQ->y + pQ->x * pQ->w );
00429 pOut->mat[13] = 0.0f;
00430
00431
00432 pOut->mat[ 2] = 2.0f * ( pQ->x * pQ->z + pQ->y * pQ->w );
00433 pOut->mat[ 6] = 2.0f * ( pQ->y * pQ->z - pQ->x * pQ->w );
00434 pOut->mat[10] = 1.0f - 2.0f * ( pQ->x * pQ->x + pQ->y * pQ->y );
00435 pOut->mat[14] = 0.0f;
00436
00437
00438 pOut->mat[ 3] = 0;
00439 pOut->mat[ 7] = 0;
00440 pOut->mat[11] = 0;
00441 pOut->mat[15] = 1.0f;
00442
00443 return pOut;
00444 }
00445
00447 kmMat4* kmMat4Scaling(kmMat4* pOut, const kmScalar x, const kmScalar y,
00448 const kmScalar z)
00449 {
00450 memset(pOut->mat, 0, sizeof(float) * 16);
00451 pOut->mat[0] = x;
00452 pOut->mat[5] = y;
00453 pOut->mat[10] = z;
00454 pOut->mat[15] = 1.0f;
00455
00456 return pOut;
00457 }
00458
00463 kmMat4* kmMat4Translation(kmMat4* pOut, const kmScalar x,
00464 const kmScalar y, const kmScalar z)
00465 {
00466
00467 memset(pOut->mat, 0, sizeof(float) * 16);
00468
00469 pOut->mat[0] = 1.0f;
00470 pOut->mat[5] = 1.0f;
00471 pOut->mat[10] = 1.0f;
00472
00473 pOut->mat[12] = x;
00474 pOut->mat[13] = y;
00475 pOut->mat[14] = z;
00476 pOut->mat[15] = 1.0f;
00477
00478 return pOut;
00479 }
00480
00486 kmVec3* kmMat4GetUpVec3(kmVec3* pOut, const kmMat4* pIn)
00487 {
00488 pOut->x = pIn->mat[4];
00489 pOut->y = pIn->mat[5];
00490 pOut->z = pIn->mat[6];
00491
00492 kmVec3Normalize(pOut, pOut);
00493
00494 return pOut;
00495 }
00496
00500 kmVec3* kmMat4GetRightVec3(kmVec3* pOut, const kmMat4* pIn)
00501 {
00502 pOut->x = pIn->mat[0];
00503 pOut->y = pIn->mat[1];
00504 pOut->z = pIn->mat[2];
00505
00506 kmVec3Normalize(pOut, pOut);
00507
00508 return pOut;
00509 }
00510
00515 kmVec3* kmMat4GetForwardVec3(kmVec3* pOut, const kmMat4* pIn)
00516 {
00517 pOut->x = pIn->mat[8];
00518 pOut->y = pIn->mat[9];
00519 pOut->z = pIn->mat[10];
00520
00521 kmVec3Normalize(pOut, pOut);
00522
00523 return pOut;
00524 }
00525
00530 kmMat4* kmMat4PerspectiveProjection(kmMat4* pOut, kmScalar fovY,
00531 kmScalar aspect, kmScalar zNear,
00532 kmScalar zFar)
00533 {
00534 kmScalar r = kmDegreesToRadians(fovY / 2);
00535 kmScalar deltaZ = zFar - zNear;
00536 kmScalar s = sin(r);
00537 kmScalar cotangent = 0;
00538
00539 if (deltaZ == 0 || s == 0 || aspect == 0) {
00540 return NULL;
00541 }
00542
00543
00544 cotangent = cos(r) / s;
00545
00546 kmMat4Identity(pOut);
00547 pOut->mat[0] = cotangent / aspect;
00548 pOut->mat[5] = cotangent;
00549 pOut->mat[10] = -(zFar + zNear) / deltaZ;
00550 pOut->mat[11] = -1;
00551 pOut->mat[14] = -2 * zNear * zFar / deltaZ;
00552 pOut->mat[15] = 0;
00553
00554 return pOut;
00555 }
00556
00558 kmMat4* kmMat4OrthographicProjection(kmMat4* pOut, kmScalar left,
00559 kmScalar right, kmScalar bottom,
00560 kmScalar top, kmScalar nearVal,
00561 kmScalar farVal)
00562 {
00563 kmScalar tx = -((right + left) / (right - left));
00564 kmScalar ty = -((top + bottom) / (top - bottom));
00565 kmScalar tz = -((farVal + nearVal) / (farVal - nearVal));
00566
00567 kmMat4Identity(pOut);
00568 pOut->mat[0] = 2 / (right - left);
00569 pOut->mat[5] = 2 / (top - bottom);
00570 pOut->mat[10] = -2 / (farVal - nearVal);
00571 pOut->mat[12] = tx;
00572 pOut->mat[13] = ty;
00573 pOut->mat[14] = tz;
00574
00575 return pOut;
00576 }
00577
00582 kmMat4* kmMat4LookAt(kmMat4* pOut, const kmVec3* pEye,
00583 const kmVec3* pCenter, const kmVec3* pUp)
00584 {
00585 kmVec3 f, up, s, u;
00586 kmMat4 translate;
00587
00588 kmVec3Subtract(&f, pCenter, pEye);
00589 kmVec3Normalize(&f, &f);
00590
00591 kmVec3Assign(&up, pUp);
00592 kmVec3Normalize(&up, &up);
00593
00594 kmVec3Cross(&s, &f, &up);
00595 kmVec3Normalize(&s, &s);
00596
00597 kmVec3Cross(&u, &s, &f);
00598 kmVec3Normalize(&s, &s);
00599
00600 kmMat4Identity(pOut);
00601
00602 pOut->mat[0] = s.x;
00603 pOut->mat[4] = s.y;
00604 pOut->mat[8] = s.z;
00605
00606 pOut->mat[1] = u.x;
00607 pOut->mat[5] = u.y;
00608 pOut->mat[9] = u.z;
00609
00610 pOut->mat[2] = -f.x;
00611 pOut->mat[6] = -f.y;
00612 pOut->mat[10] = -f.z;
00613
00614 kmMat4Translation(&translate, -pEye->x, -pEye->y, -pEye->z);
00615 kmMat4Multiply(pOut, pOut, &translate);
00616
00617 return pOut;
00618 }