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
00026
00027 #include <assert.h>
00028 #include <memory.h>
00029
00030 #include "utility.h"
00031 #include "mat4.h"
00032 #include "vec3.h"
00033 #include "quaternion.h"
00034
00036 kmQuaternion* kmQuaternionConjugate(kmQuaternion* pOut, const kmQuaternion* pIn)
00037 {
00038 pOut->x = -pIn->x;
00039 pOut->y = -pIn->y;
00040 pOut->z = -pIn->z;
00041 pOut->w = pIn->w;
00042
00043 return pOut;
00044 }
00045
00047 kmScalar kmQuaternionDot(const kmQuaternion* q1, const kmQuaternion* q2)
00048 {
00049
00050
00051 return (q1->w * q2->w +
00052 q1->x * q2->x +
00053 q1->y * q2->y +
00054 q1->z * q2->z);
00055 }
00056
00058 kmQuaternion* kmQuaternionExp(kmQuaternion* pOut, const kmQuaternion* pIn)
00059 {
00060 assert(0);
00061
00062 return pOut;
00063 }
00064
00066 kmQuaternion* kmQuaternionIdentity(kmQuaternion* pOut)
00067 {
00068 pOut->x = 0.0;
00069 pOut->y = 0.0;
00070 pOut->z = 0.0;
00071 pOut->w = 1.0;
00072
00073 return pOut;
00074 }
00075
00077 kmQuaternion* kmQuaternionInverse(kmQuaternion* pOut,
00078 const kmQuaternion* pIn)
00079 {
00080 kmScalar l = kmQuaternionLength(pIn);
00081 kmQuaternion tmp;
00082
00083 if (fabs(l) > kmEpsilon)
00084 {
00085 pOut->x = 0.0;
00086 pOut->y = 0.0;
00087 pOut->z = 0.0;
00088 pOut->w = 0.0;
00089
00090 return pOut;
00091 }
00092
00093
00094
00096 kmQuaternionScale(pOut,
00097 kmQuaternionConjugate(&tmp, pIn), 1.0f / l);
00098
00099 return pOut;
00100 }
00101
00103 int kmQuaternionIsIdentity(const kmQuaternion* pIn)
00104 {
00105 return (pIn->x == 0.0 && pIn->y == 0.0 && pIn->z == 0.0 &&
00106 pIn->w == 1.0);
00107 }
00108
00110 kmScalar kmQuaternionLength(const kmQuaternion* pIn)
00111 {
00112 return sqrtf(kmQuaternionLengthSq(pIn));
00113 }
00114
00116 kmScalar kmQuaternionLengthSq(const kmQuaternion* pIn)
00117 {
00118 return pIn->x * pIn->x + pIn->y * pIn->y +
00119 pIn->z * pIn->z + pIn->w * pIn->w;
00120 }
00121
00123 kmQuaternion* kmQuaternionLn(kmQuaternion* pOut,
00124 const kmQuaternion* pIn)
00125 {
00126
00127
00128
00129
00130
00131
00132 assert(0);
00133
00134 return pOut;
00135 }
00136
00138 extern
00139 kmQuaternion* kmQuaternionMultiply(kmQuaternion* pOut,
00140 const kmQuaternion* q1,
00141 const kmQuaternion* q2)
00142 {
00143 pOut->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
00144 pOut->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
00145 pOut->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
00146 pOut->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
00147
00148 return pOut;
00149 }
00150
00152 kmQuaternion* kmQuaternionNormalize(kmQuaternion* pOut,
00153 const kmQuaternion* pIn)
00154 {
00155 kmScalar length = kmQuaternionLength(pIn);
00156 assert(fabs(length) > kmEpsilon);
00157 kmQuaternionScale(pOut, pIn, 1.0f / length);
00158
00159 return pOut;
00160 }
00161
00163 kmQuaternion* kmQuaternionRotationAxis(kmQuaternion* pOut,
00164 const kmVec3* pV,
00165 kmScalar angle)
00166 {
00167 kmScalar rad = angle * 0.5f;
00168 kmScalar scale = sinf(rad);
00169
00170 pOut->w = cosf(rad);
00171 pOut->x = pV->x * scale;
00172 pOut->y = pV->y * scale;
00173 pOut->z = pV->z * scale;
00174
00175 return pOut;
00176 }
00177
00179 kmQuaternion* kmQuaternionRotationMatrix(kmQuaternion* pOut,
00180 const kmMat4* pIn)
00181 {
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207 kmScalar T = pIn->mat[0] + pIn->mat[5] + pIn->mat[10];
00208
00209 if (T > kmEpsilon) {
00210
00211
00212
00213
00214
00215
00216
00217 kmScalar s = sqrtf(T) * 2;
00218 pOut->x = (pIn->mat[9] - pIn->mat[6]) / s;
00219 pOut->y = (pIn->mat[8] - pIn->mat[2]) / s;
00220 pOut->z = (pIn->mat[1] - pIn->mat[4]) / s;
00221 pOut->w = 0.25f * s;
00222
00223 kmQuaternionNormalize(pOut, pOut);
00224 return pOut;
00225 }
00226
00227
00228
00229 if (pIn->mat[0] > pIn->mat[5] && pIn->mat[0] > pIn->mat[10]) {
00230 kmScalar s = sqrtf(1 + pIn->mat[0] - pIn->mat[5] - pIn->mat[10]) * 2;
00231 pOut->x = 0.25f * s;
00232 pOut->y = (pIn->mat[1] + pIn->mat[4]) / s;
00233 pOut->z = (pIn->mat[8] + pIn->mat[2]) / s;
00234 pOut->w = (pIn->mat[9] - pIn->mat[6]) / s;
00235 }
00236 else if (pIn->mat[5] > pIn->mat[10]) {
00237 kmScalar s = sqrtf(1 + pIn->mat[5] - pIn->mat[0] - pIn->mat[10]) * 2;
00238 pOut->x = (pIn->mat[1] + pIn->mat[4]) / s;
00239 pOut->y = 0.25f * s;
00240 pOut->z = (pIn->mat[9] + pIn->mat[6]) / s;
00241 pOut->w = (pIn->mat[8] - pIn->mat[2]) / s;
00242 }
00243 else {
00244 kmScalar s = sqrt(1.0f + pIn->mat[10] - pIn->mat[0] - pIn->mat[5]) * 2.0f;
00245 pOut->x = (pIn->mat[8] + pIn->mat[2] ) / s;
00246 pOut->y = (pIn->mat[6] + pIn->mat[9] ) / s;
00247 pOut->z = 0.25f * s;
00248 pOut->w = (pIn->mat[1] - pIn->mat[4] ) / s;
00249 }
00250
00251 kmQuaternionNormalize(pOut, pOut);
00252 return pOut;
00253 }
00254
00256 kmQuaternion* kmQuaternionRotationYawPitchRoll(kmQuaternion* pOut,
00257 kmScalar yaw,
00258 kmScalar pitch,
00259 kmScalar roll)
00260 {
00261 kmScalar ex, ey, ez;
00262 kmScalar cr, cp, cy, sr, sp, sy, cpcy, spsy;
00263
00264 ex = kmDegreesToRadians(pitch) / 2.0f;
00265 ey = kmDegreesToRadians(yaw) / 2.0f;
00266 ez = kmDegreesToRadians(roll) / 2.0f;
00267
00268 cr = cosf(ex);
00269 cp = cosf(ey);
00270 cy = cosf(ez);
00271
00272 sr = sinf(ex);
00273 sp = sinf(ey);
00274 sy = sinf(ez);
00275
00276 cpcy = cp * cy;
00277 spsy = sp * sy;
00278
00279 pOut->w = cr * cpcy + sr * spsy;
00280
00281 pOut->x = sr * cpcy - cr * spsy;
00282 pOut->y = cr * sp * cy + sr * cp * sy;
00283 pOut->z = cr * cp * sy - sr * sp * cy;
00284
00285 kmQuaternionNormalize(pOut, pOut);
00286
00287 return pOut;
00288 }
00289
00291 kmQuaternion* kmQuaternionSlerp(kmQuaternion* pOut,
00292 const kmQuaternion* q1,
00293 const kmQuaternion* q2,
00294 kmScalar t)
00295 {
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309 kmScalar ct = kmQuaternionDot(q1, q2);
00310 kmScalar theta = acosf(ct);
00311 kmScalar st = sqrtf(1.0 - kmSQR(ct));
00312
00313 kmScalar stt = sinf(t * theta) / st;
00314 kmScalar somt = sinf((1.0 - t) * theta) / st;
00315
00316 kmQuaternion temp, temp2;
00317 kmQuaternionScale(&temp, q1, somt);
00318 kmQuaternionScale(&temp2, q2, stt);
00319 kmQuaternionAdd(pOut, &temp, &temp2);
00320
00321 return pOut;
00322 }
00323
00325 void kmQuaternionToAxisAngle(const kmQuaternion* pIn,
00326 kmVec3* pAxis,
00327 kmScalar* pAngle)
00328 {
00329 kmScalar tempAngle;
00330 kmScalar scale;
00331
00332 tempAngle = acosf(pIn->w);
00333 scale = sqrtf(kmSQR(pIn->x) + kmSQR(pIn->y) + kmSQR(pIn->z));
00334
00335 if (((scale > -kmEpsilon) && scale < kmEpsilon)
00336 || (scale < 360.0f + kmEpsilon && scale > 360.0f - kmEpsilon))
00337 {
00338 *pAngle = 0.0f;
00339
00340 pAxis->x = 0.0f;
00341 pAxis->y = 0.0f;
00342 pAxis->z = 1.0f;
00343 }
00344 else
00345 {
00346 *pAngle = tempAngle * 2.0f;
00347
00348 pAxis->x = pIn->x / scale;
00349 pAxis->y = pIn->y / scale;
00350 pAxis->z = pIn->z / scale;
00351 kmVec3Normalize(pAxis, pAxis);
00352 }
00353 }
00354
00355 kmQuaternion* kmQuaternionScale(kmQuaternion* pOut,
00356 const kmQuaternion* pIn,
00357 kmScalar s)
00358 {
00359 pOut->x = pIn->x * s;
00360 pOut->y = pIn->y * s;
00361 pOut->z = pIn->z * s;
00362 pOut->w = pIn->w * s;
00363
00364 return pOut;
00365 }
00366
00367 kmQuaternion* kmQuaternionAssign(kmQuaternion* pOut, const kmQuaternion* pIn)
00368 {
00369 memcpy(pOut, pIn, sizeof(float) * 4);
00370
00371 return pOut;
00372 }
00373
00374 kmQuaternion* kmQuaternionAdd(kmQuaternion* pOut, const kmQuaternion* pQ1, const kmQuaternion* pQ2)
00375 {
00376 pOut->x = pQ1->x + pQ2->x;
00377 pOut->y = pQ1->y + pQ2->y;
00378 pOut->z = pQ1->z + pQ2->z;
00379 pOut->w = pQ1->w + pQ2->w;
00380
00381 return pOut;
00382 }
00383
00395 kmQuaternion* kmQuaternionRotationBetweenVec3(kmQuaternion* pOut, const kmVec3* vec1, const kmVec3* vec2, const kmVec3* fallback) {
00396
00397 kmVec3 v1, v2;
00398 kmScalar a;
00399
00400 kmVec3Assign(&v1, vec1);
00401 kmVec3Assign(&v2, vec2);
00402
00403 kmVec3Normalize(&v1, &v1);
00404 kmVec3Normalize(&v2, &v2);
00405
00406 a = kmVec3Dot(&v1, &v2);
00407
00408 if (a >= 1.0) {
00409 kmQuaternionIdentity(pOut);
00410 return pOut;
00411 }
00412
00413 if (a < (1e-6f - 1.0f)) {
00414 if (fabs(kmVec3LengthSq(fallback)) < kmEpsilon) {
00415 kmQuaternionRotationAxis(pOut, fallback, kmPI);
00416 } else {
00417 kmVec3 axis;
00418 kmVec3 X;
00419 X.x = 1.0;
00420 X.y = 0.0;
00421 X.z = 0.0;
00422
00423
00424 kmVec3Cross(&axis, &X, vec1);
00425
00426
00427 if (fabs(kmVec3LengthSq(&axis)) < kmEpsilon) {
00428 kmVec3 Y;
00429 Y.x = 0.0;
00430 Y.y = 1.0;
00431 Y.z = 0.0;
00432
00433 kmVec3Cross(&axis, &Y, vec1);
00434 }
00435
00436 kmVec3Normalize(&axis, &axis);
00437
00438 kmQuaternionRotationAxis(pOut, &axis, kmPI);
00439 }
00440 } else {
00441 kmScalar s = sqrtf((1+a) * 2);
00442 kmScalar invs = 1 / s;
00443
00444 kmVec3 c;
00445 kmVec3Cross(&c, &v1, &v2);
00446
00447 pOut->x = c.x * invs;
00448 pOut->y = c.y * invs;
00449 pOut->z = c.z * invs;
00450 pOut->w = s * 0.5f;
00451
00452 kmQuaternionNormalize(pOut, pOut);
00453 }
00454
00455 return pOut;
00456
00457 }
00458
00459 kmVec3* kmQuaternionMultiplyVec3(kmVec3* pOut, const kmQuaternion* q, const kmVec3* v) {
00460 kmVec3 uv, uuv, qvec;
00461
00462 qvec.x = q->x;
00463 qvec.y = q->y;
00464 qvec.z = q->z;
00465
00466 kmVec3Cross(&uv, &qvec, v);
00467 kmVec3Cross(&uuv, &qvec, &uv);
00468
00469 kmVec3Scale(&uv, &uv, (2.0f * q->w));
00470 kmVec3Scale(&uuv, &uuv, 2.0f);
00471
00472 kmVec3Add(pOut, v, &uv);
00473 kmVec3Add(pOut, pOut, &uuv);
00474
00475 return pOut;
00476 }
00477