00001
00032 #include "linden_common.h"
00033
00034 #include "llquaternion.h"
00035
00036 #include "llmath.h"
00037
00038 #include "v3math.h"
00039 #include "v3dmath.h"
00040 #include "v4math.h"
00041 #include "m4math.h"
00042 #include "m3math.h"
00043 #include "llquantize.h"
00044
00045
00046
00047 const LLQuaternion LLQuaternion::DEFAULT;
00048
00049
00050
00051 LLQuaternion::LLQuaternion(const LLMatrix4 &mat)
00052 {
00053 *this = mat.quaternion();
00054 normalize();
00055 }
00056
00057 LLQuaternion::LLQuaternion(const LLMatrix3 &mat)
00058 {
00059 *this = mat.quaternion();
00060 normalize();
00061 }
00062
00063 LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec)
00064 {
00065 LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
00066 v.normalize();
00067
00068 F32 c, s;
00069 c = cosf(angle*0.5f);
00070 s = sinf(angle*0.5f);
00071
00072 mQ[VX] = v.mV[VX] * s;
00073 mQ[VY] = v.mV[VY] * s;
00074 mQ[VZ] = v.mV[VZ] * s;
00075 mQ[VW] = c;
00076 normalize();
00077 }
00078
00079 LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
00080 {
00081 LLVector3 v(vec);
00082 v.normalize();
00083
00084 F32 c, s;
00085 c = cosf(angle*0.5f);
00086 s = sinf(angle*0.5f);
00087
00088 mQ[VX] = v.mV[VX] * s;
00089 mQ[VY] = v.mV[VY] * s;
00090 mQ[VZ] = v.mV[VZ] * s;
00091 mQ[VW] = c;
00092 normalize();
00093 }
00094
00095 LLQuaternion::LLQuaternion(const LLVector3 &x_axis,
00096 const LLVector3 &y_axis,
00097 const LLVector3 &z_axis)
00098 {
00099 LLMatrix3 mat;
00100 mat.setRows(x_axis, y_axis, z_axis);
00101 *this = mat.quaternion();
00102 normalize();
00103 }
00104
00105
00106 void LLQuaternion::quantize16(F32 lower, F32 upper)
00107 {
00108 F32 x = mQ[VX];
00109 F32 y = mQ[VY];
00110 F32 z = mQ[VZ];
00111 F32 s = mQ[VS];
00112
00113 x = U16_to_F32(F32_to_U16_ROUND(x, lower, upper), lower, upper);
00114 y = U16_to_F32(F32_to_U16_ROUND(y, lower, upper), lower, upper);
00115 z = U16_to_F32(F32_to_U16_ROUND(z, lower, upper), lower, upper);
00116 s = U16_to_F32(F32_to_U16_ROUND(s, lower, upper), lower, upper);
00117
00118 mQ[VX] = x;
00119 mQ[VY] = y;
00120 mQ[VZ] = z;
00121 mQ[VS] = s;
00122
00123 normQuat();
00124 }
00125
00126 void LLQuaternion::quantize8(F32 lower, F32 upper)
00127 {
00128 mQ[VX] = U8_to_F32(F32_to_U8_ROUND(mQ[VX], lower, upper), lower, upper);
00129 mQ[VY] = U8_to_F32(F32_to_U8_ROUND(mQ[VY], lower, upper), lower, upper);
00130 mQ[VZ] = U8_to_F32(F32_to_U8_ROUND(mQ[VZ], lower, upper), lower, upper);
00131 mQ[VS] = U8_to_F32(F32_to_U8_ROUND(mQ[VS], lower, upper), lower, upper);
00132
00133 normQuat();
00134 }
00135
00136
00137
00138
00139
00140
00141 const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z)
00142 {
00143 LLVector3 vec(x, y, z);
00144 vec.normalize();
00145
00146 angle *= 0.5f;
00147 F32 c, s;
00148 c = cosf(angle);
00149 s = sinf(angle);
00150
00151 mQ[VX] = vec.mV[VX]*s;
00152 mQ[VY] = vec.mV[VY]*s;
00153 mQ[VZ] = vec.mV[VZ]*s;
00154 mQ[VW] = c;
00155
00156 normalize();
00157 return (*this);
00158 }
00159
00160 const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
00161 {
00162 LLVector3 v(vec);
00163 v.normalize();
00164
00165 angle *= 0.5f;
00166 F32 c, s;
00167 c = cosf(angle);
00168 s = sinf(angle);
00169
00170 mQ[VX] = v.mV[VX]*s;
00171 mQ[VY] = v.mV[VY]*s;
00172 mQ[VZ] = v.mV[VZ]*s;
00173 mQ[VW] = c;
00174
00175 normalize();
00176 return (*this);
00177 }
00178
00179 const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
00180 {
00181 LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
00182 v.normalize();
00183
00184 F32 c, s;
00185 c = cosf(angle*0.5f);
00186 s = sinf(angle*0.5f);
00187
00188 mQ[VX] = v.mV[VX]*s;
00189 mQ[VY] = v.mV[VY]*s;
00190 mQ[VZ] = v.mV[VZ]*s;
00191 mQ[VW] = c;
00192
00193 normalize();
00194 return (*this);
00195 }
00196
00197 const LLQuaternion& LLQuaternion::setEulerAngles(F32 roll, F32 pitch, F32 yaw)
00198 {
00199 LLMatrix3 rot_mat(roll, pitch, yaw);
00200 rot_mat.orthogonalize();
00201 *this = rot_mat.quaternion();
00202
00203 normalize();
00204 return (*this);
00205 }
00206
00207
00208 const LLQuaternion& LLQuaternion::set(const LLMatrix3 &mat)
00209 {
00210 *this = mat.quaternion();
00211 normalize();
00212 return (*this);
00213 }
00214
00215
00216 const LLQuaternion& LLQuaternion::set(const LLMatrix4 &mat)
00217 {
00218 *this = mat.quaternion();
00219 normalize();
00220 return (*this);
00221 }
00222
00223
00224 const LLQuaternion& LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z)
00225 {
00226 LLVector3 vec(x, y, z);
00227 vec.normalize();
00228
00229 angle *= 0.5f;
00230 F32 c, s;
00231 c = cosf(angle);
00232 s = sinf(angle);
00233
00234 mQ[VX] = vec.mV[VX]*s;
00235 mQ[VY] = vec.mV[VY]*s;
00236 mQ[VZ] = vec.mV[VZ]*s;
00237 mQ[VW] = c;
00238
00239 normalize();
00240 return (*this);
00241 }
00242
00243
00244 const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
00245 {
00246 LLVector3 v(vec);
00247 v.normalize();
00248
00249 angle *= 0.5f;
00250 F32 c, s;
00251 c = cosf(angle);
00252 s = sinf(angle);
00253
00254 mQ[VX] = v.mV[VX]*s;
00255 mQ[VY] = v.mV[VY]*s;
00256 mQ[VZ] = v.mV[VZ]*s;
00257 mQ[VW] = c;
00258
00259 normalize();
00260 return (*this);
00261 }
00262
00263 const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
00264 {
00265 LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
00266 v.normalize();
00267
00268 F32 c, s;
00269 c = cosf(angle*0.5f);
00270 s = sinf(angle*0.5f);
00271
00272 mQ[VX] = v.mV[VX]*s;
00273 mQ[VY] = v.mV[VY]*s;
00274 mQ[VZ] = v.mV[VZ]*s;
00275 mQ[VW] = c;
00276
00277 normalize();
00278 return (*this);
00279 }
00280
00281 const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
00282 {
00283 LLMatrix3 rot_mat(roll, pitch, yaw);
00284 rot_mat.orthogonalize();
00285 *this = rot_mat.quaternion();
00286
00287 normalize();
00288 return (*this);
00289 }
00290
00291 const LLQuaternion& LLQuaternion::setQuat(const LLMatrix3 &mat)
00292 {
00293 *this = mat.quaternion();
00294 normalize();
00295 return (*this);
00296 }
00297
00298 const LLQuaternion& LLQuaternion::setQuat(const LLMatrix4 &mat)
00299 {
00300 *this = mat.quaternion();
00301 normalize();
00302 return (*this);
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350 }
00351
00352
00353
00354
00355
00356
00357 LLMatrix3 LLQuaternion::getMatrix3(void) const
00358 {
00359 LLMatrix3 mat;
00360 F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
00361
00362 xx = mQ[VX] * mQ[VX];
00363 xy = mQ[VX] * mQ[VY];
00364 xz = mQ[VX] * mQ[VZ];
00365 xw = mQ[VX] * mQ[VW];
00366
00367 yy = mQ[VY] * mQ[VY];
00368 yz = mQ[VY] * mQ[VZ];
00369 yw = mQ[VY] * mQ[VW];
00370
00371 zz = mQ[VZ] * mQ[VZ];
00372 zw = mQ[VZ] * mQ[VW];
00373
00374 mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz );
00375 mat.mMatrix[0][1] = 2.f * ( xy + zw );
00376 mat.mMatrix[0][2] = 2.f * ( xz - yw );
00377
00378 mat.mMatrix[1][0] = 2.f * ( xy - zw );
00379 mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz );
00380 mat.mMatrix[1][2] = 2.f * ( yz + xw );
00381
00382 mat.mMatrix[2][0] = 2.f * ( xz + yw );
00383 mat.mMatrix[2][1] = 2.f * ( yz - xw );
00384 mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy );
00385
00386 return mat;
00387 }
00388
00389 LLMatrix4 LLQuaternion::getMatrix4(void) const
00390 {
00391 LLMatrix4 mat;
00392 F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
00393
00394 xx = mQ[VX] * mQ[VX];
00395 xy = mQ[VX] * mQ[VY];
00396 xz = mQ[VX] * mQ[VZ];
00397 xw = mQ[VX] * mQ[VW];
00398
00399 yy = mQ[VY] * mQ[VY];
00400 yz = mQ[VY] * mQ[VZ];
00401 yw = mQ[VY] * mQ[VW];
00402
00403 zz = mQ[VZ] * mQ[VZ];
00404 zw = mQ[VZ] * mQ[VW];
00405
00406 mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz );
00407 mat.mMatrix[0][1] = 2.f * ( xy + zw );
00408 mat.mMatrix[0][2] = 2.f * ( xz - yw );
00409
00410 mat.mMatrix[1][0] = 2.f * ( xy - zw );
00411 mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz );
00412 mat.mMatrix[1][2] = 2.f * ( yz + xw );
00413
00414 mat.mMatrix[2][0] = 2.f * ( xz + yw );
00415 mat.mMatrix[2][1] = 2.f * ( yz - xw );
00416 mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy );
00417
00418
00419
00420 return mat;
00421 }
00422
00423
00424
00425
00426
00427
00428
00429
00430 void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b)
00431 {
00432
00433 LLVector3 vec_a = a;
00434 LLVector3 vec_b = b;
00435
00436
00437
00438 F32 vec_a_mag = vec_a.normalize();
00439 F32 vec_b_mag = vec_b.normalize();
00440 if (vec_a_mag < F_APPROXIMATELY_ZERO ||
00441 vec_b_mag < F_APPROXIMATELY_ZERO)
00442 {
00443
00444
00445 loadIdentity();
00446 return;
00447 }
00448
00449
00450 LLVector3 axis = vec_a % vec_b;
00451 F32 cos_theta = vec_a * vec_b;
00452
00453
00454 if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
00455 {
00456
00457 loadIdentity();
00458 }
00459 else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
00460 {
00461
00462
00463
00464
00465
00466 LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
00467 LLVector3 ortho_axis(1.f, 0.f, 0.f);
00468 ortho_axis -= proj;
00469
00470
00471 F32 ortho_length = ortho_axis.normalize();
00472
00473
00474 if (ortho_length < F_APPROXIMATELY_ZERO)
00475 {
00476
00477 ortho_axis.setVec(0.f, 0.f, 1.f);
00478 }
00479
00480
00481 mQ[VX] = ortho_axis.mV[VX];
00482 mQ[VY] = ortho_axis.mV[VY];
00483 mQ[VZ] = ortho_axis.mV[VZ];
00484 mQ[VW] = 0.f;
00485 }
00486 else
00487 {
00488
00489
00490 F32 theta = (F32)acos(cos_theta);
00491
00492 setAngleAxis(theta, axis);
00493 }
00494 }
00495
00496
00497 const LLQuaternion &LLQuaternion::constrain(F32 radians)
00498 {
00499 const F32 cos_angle_lim = cosf( radians/2 );
00500 const F32 sin_angle_lim = sinf( radians/2 );
00501
00502 if (mQ[VW] < 0.f)
00503 {
00504 mQ[VX] *= -1.f;
00505 mQ[VY] *= -1.f;
00506 mQ[VZ] *= -1.f;
00507 mQ[VW] *= -1.f;
00508 }
00509
00510
00511 if( mQ[VW] < cos_angle_lim )
00512 {
00513 mQ[VW] = cos_angle_lim;
00514 F32 axis_len = sqrtf( mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] );
00515 F32 axis_mult_fact = sin_angle_lim / axis_len;
00516 mQ[VX] *= axis_mult_fact;
00517 mQ[VY] *= axis_mult_fact;
00518 mQ[VZ] *= axis_mult_fact;
00519 }
00520
00521 return *this;
00522 }
00523
00524
00525
00526 std::ostream& operator<<(std::ostream &s, const LLQuaternion &a)
00527 {
00528 s << "{ "
00529 << a.mQ[VX] << ", " << a.mQ[VY] << ", " << a.mQ[VZ] << ", " << a.mQ[VW]
00530 << " }";
00531 return s;
00532 }
00533
00534
00535
00536 LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b)
00537 {
00538
00539
00540 LLQuaternion q(
00541 b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
00542 b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
00543 b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
00544 b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
00545 );
00546 return q;
00547 }
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559 LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot)
00560 {
00561 F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
00562 F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
00563 F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
00564 F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
00565
00566 F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
00567 F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
00568 F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
00569
00570 return LLVector4(nx, ny, nz, a.mV[VW]);
00571 }
00572
00573 LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot)
00574 {
00575 F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
00576 F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
00577 F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
00578 F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
00579
00580 F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
00581 F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
00582 F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
00583
00584 return LLVector3(nx, ny, nz);
00585 }
00586
00587 LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot)
00588 {
00589 F64 rw = - rot.mQ[VX] * a.mdV[VX] - rot.mQ[VY] * a.mdV[VY] - rot.mQ[VZ] * a.mdV[VZ];
00590 F64 rx = rot.mQ[VW] * a.mdV[VX] + rot.mQ[VY] * a.mdV[VZ] - rot.mQ[VZ] * a.mdV[VY];
00591 F64 ry = rot.mQ[VW] * a.mdV[VY] + rot.mQ[VZ] * a.mdV[VX] - rot.mQ[VX] * a.mdV[VZ];
00592 F64 rz = rot.mQ[VW] * a.mdV[VZ] + rot.mQ[VX] * a.mdV[VY] - rot.mQ[VY] * a.mdV[VX];
00593
00594 F64 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
00595 F64 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
00596 F64 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
00597
00598 return LLVector3d(nx, ny, nz);
00599 }
00600
00601 F32 dot(const LLQuaternion &a, const LLQuaternion &b)
00602 {
00603 return a.mQ[VX] * b.mQ[VX] +
00604 a.mQ[VY] * b.mQ[VY] +
00605 a.mQ[VZ] * b.mQ[VZ] +
00606 a.mQ[VW] * b.mQ[VW];
00607 }
00608
00609
00610
00611 #if 0
00612
00613 LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
00614 {
00615 LLQuaternion r;
00616 r = t * (q - p) + p;
00617 r.normalize();
00618 return r;
00619 }
00620 #endif
00621
00622
00623 LLQuaternion lerp(F32 t, const LLQuaternion &q)
00624 {
00625 LLQuaternion r;
00626 r.mQ[VX] = t * q.mQ[VX];
00627 r.mQ[VY] = t * q.mQ[VY];
00628 r.mQ[VZ] = t * q.mQ[VZ];
00629 r.mQ[VW] = t * (q.mQ[VZ] - 1.f) + 1.f;
00630 r.normalize();
00631 return r;
00632 }
00633
00634 LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
00635 {
00636 LLQuaternion r;
00637 F32 inv_t;
00638
00639 inv_t = 1.f - t;
00640
00641 r.mQ[VX] = t * q.mQ[VX] + (inv_t * p.mQ[VX]);
00642 r.mQ[VY] = t * q.mQ[VY] + (inv_t * p.mQ[VY]);
00643 r.mQ[VZ] = t * q.mQ[VZ] + (inv_t * p.mQ[VZ]);
00644 r.mQ[VW] = t * q.mQ[VW] + (inv_t * p.mQ[VW]);
00645 r.normalize();
00646 return r;
00647 }
00648
00649
00650
00651 LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b )
00652 {
00653
00654 F32 cos_t = a.mQ[0]*b.mQ[0] + a.mQ[1]*b.mQ[1] + a.mQ[2]*b.mQ[2] + a.mQ[3]*b.mQ[3];
00655
00656
00657 int bflip;
00658 if (cos_t < 0.0f)
00659 {
00660 cos_t = -cos_t;
00661 bflip = TRUE;
00662 }
00663 else
00664 bflip = FALSE;
00665
00666
00667
00668 F32 alpha;
00669 F32 beta;
00670 if (1.0f - cos_t < 0.00001f)
00671 {
00672 beta = 1.0f - u;
00673 alpha = u;
00674 }
00675 else
00676 {
00677 F32 theta = acosf(cos_t);
00678 F32 sin_t = sinf(theta);
00679 beta = sinf(theta - u*theta) / sin_t;
00680 alpha = sinf(u*theta) / sin_t;
00681 }
00682
00683 if (bflip)
00684 beta = -beta;
00685
00686
00687 LLQuaternion ret;
00688 ret.mQ[0] = beta*a.mQ[0] + alpha*b.mQ[0];
00689 ret.mQ[1] = beta*a.mQ[1] + alpha*b.mQ[1];
00690 ret.mQ[2] = beta*a.mQ[2] + alpha*b.mQ[2];
00691 ret.mQ[3] = beta*a.mQ[3] + alpha*b.mQ[3];
00692
00693 return ret;
00694 }
00695
00696
00697 LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b)
00698 {
00699 if (dot(a, b) < 0.f)
00700 {
00701 return slerp(t, a, b);
00702 }
00703 else
00704 {
00705 return lerp(t, a, b);
00706 }
00707 }
00708
00709 LLQuaternion nlerp(F32 t, const LLQuaternion &q)
00710 {
00711 if (q.mQ[VW] < 0.f)
00712 {
00713 return slerp(t, q);
00714 }
00715 else
00716 {
00717 return lerp(t, q);
00718 }
00719 }
00720
00721
00722 LLQuaternion slerp(F32 t, const LLQuaternion &q)
00723 {
00724 F32 c = q.mQ[VW];
00725 if (1.0f == t || 1.0f == c)
00726 {
00727
00728 return q;
00729 }
00730
00731 LLQuaternion r;
00732 F32 s, angle, stq, stp;
00733
00734 s = (F32) sqrt(1.f - c*c);
00735
00736 if (c < 0.0f)
00737 {
00738
00739
00740
00741
00742
00743 angle = (F32) acos(-c);
00744 stp = -(F32) sin(angle * (1.f - t));
00745 stq = (F32) sin(angle * t);
00746 }
00747 else
00748 {
00749 angle = (F32) acos(c);
00750 stp = (F32) sin(angle * (1.f - t));
00751 stq = (F32) sin(angle * t);
00752 }
00753
00754 r.mQ[VX] = (q.mQ[VX] * stq) / s;
00755 r.mQ[VY] = (q.mQ[VY] * stq) / s;
00756 r.mQ[VZ] = (q.mQ[VZ] * stq) / s;
00757 r.mQ[VW] = (stp + q.mQ[VW] * stq) / s;
00758
00759 return r;
00760 }
00761
00762 LLQuaternion mayaQ(F32 xRot, F32 yRot, F32 zRot, LLQuaternion::Order order)
00763 {
00764 LLQuaternion xQ( xRot*DEG_TO_RAD, LLVector3(1.0f, 0.0f, 0.0f) );
00765 LLQuaternion yQ( yRot*DEG_TO_RAD, LLVector3(0.0f, 1.0f, 0.0f) );
00766 LLQuaternion zQ( zRot*DEG_TO_RAD, LLVector3(0.0f, 0.0f, 1.0f) );
00767 LLQuaternion ret;
00768 switch( order )
00769 {
00770 case LLQuaternion::XYZ:
00771 ret = xQ * yQ * zQ;
00772 break;
00773 case LLQuaternion::YZX:
00774 ret = yQ * zQ * xQ;
00775 break;
00776 case LLQuaternion::ZXY:
00777 ret = zQ * xQ * yQ;
00778 break;
00779 case LLQuaternion::XZY:
00780 ret = xQ * zQ * yQ;
00781 break;
00782 case LLQuaternion::YXZ:
00783 ret = yQ * xQ * zQ;
00784 break;
00785 case LLQuaternion::ZYX:
00786 ret = zQ * yQ * xQ;
00787 break;
00788 }
00789 return ret;
00790 }
00791
00792 const char *OrderToString( const LLQuaternion::Order order )
00793 {
00794 char *p = NULL;
00795 switch( order )
00796 {
00797 default:
00798 case LLQuaternion::XYZ:
00799 p = "XYZ";
00800 break;
00801 case LLQuaternion::YZX:
00802 p = "YZX";
00803 break;
00804 case LLQuaternion::ZXY:
00805 p = "ZXY";
00806 break;
00807 case LLQuaternion::XZY:
00808 p = "XZY";
00809 break;
00810 case LLQuaternion::YXZ:
00811 p = "YXZ";
00812 break;
00813 case LLQuaternion::ZYX:
00814 p = "ZYX";
00815 break;
00816 }
00817 return p;
00818 }
00819
00820 LLQuaternion::Order StringToOrder( const char *str )
00821 {
00822 if (strncmp(str, "XYZ", 3)==0 || strncmp(str, "xyz", 3)==0)
00823 return LLQuaternion::XYZ;
00824
00825 if (strncmp(str, "YZX", 3)==0 || strncmp(str, "yzx", 3)==0)
00826 return LLQuaternion::YZX;
00827
00828 if (strncmp(str, "ZXY", 3)==0 || strncmp(str, "zxy", 3)==0)
00829 return LLQuaternion::ZXY;
00830
00831 if (strncmp(str, "XZY", 3)==0 || strncmp(str, "xzy", 3)==0)
00832 return LLQuaternion::XZY;
00833
00834 if (strncmp(str, "YXZ", 3)==0 || strncmp(str, "yxz", 3)==0)
00835 return LLQuaternion::YXZ;
00836
00837 if (strncmp(str, "ZYX", 3)==0 || strncmp(str, "zyx", 3)==0)
00838 return LLQuaternion::ZYX;
00839
00840 return LLQuaternion::XYZ;
00841 }
00842
00843 void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const
00844 {
00845 F32 cos_a = mQ[VW];
00846 if (cos_a > 1.0f) cos_a = 1.0f;
00847 if (cos_a < -1.0f) cos_a = -1.0f;
00848
00849 F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
00850
00851 if ( fabs( sin_a ) < 0.0005f )
00852 sin_a = 1.0f;
00853 else
00854 sin_a = 1.f/sin_a;
00855
00856 F32 temp_angle = 2.0f * (F32) acos( cos_a );
00857 if (temp_angle > F_PI)
00858 {
00859
00860
00861
00862
00863
00864
00865
00866 *angle = 2.f * F_PI - temp_angle;
00867 vec.mV[VX] = - mQ[VX] * sin_a;
00868 vec.mV[VY] = - mQ[VY] * sin_a;
00869 vec.mV[VZ] = - mQ[VZ] * sin_a;
00870 }
00871 else
00872 {
00873 *angle = temp_angle;
00874 vec.mV[VX] = mQ[VX] * sin_a;
00875 vec.mV[VY] = mQ[VY] * sin_a;
00876 vec.mV[VZ] = mQ[VZ] * sin_a;
00877 }
00878 }
00879
00880
00881
00882 void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
00883 {
00884 LLMatrix3 rot_mat(*this);
00885 rot_mat.orthogonalize();
00886 rot_mat.getEulerAngles(roll, pitch, yaw);
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906 }
00907
00908
00909 LLVector3 LLQuaternion::packToVector3() const
00910 {
00911 if( mQ[VW] >= 0 )
00912 {
00913 return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
00914 }
00915 else
00916 {
00917 return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
00918 }
00919 }
00920
00921
00922 void LLQuaternion::unpackFromVector3( const LLVector3& vec )
00923 {
00924 mQ[VX] = vec.mV[VX];
00925 mQ[VY] = vec.mV[VY];
00926 mQ[VZ] = vec.mV[VZ];
00927 F32 t = 1.f - vec.magVecSquared();
00928 if( t > 0 )
00929 {
00930 mQ[VW] = sqrt( t );
00931 }
00932 else
00933 {
00934
00935
00936 mQ[VW] = 0;
00937 }
00938 }
00939
00940 BOOL LLQuaternion::parseQuat(const char* buf, LLQuaternion* value)
00941 {
00942 if( buf == NULL || buf[0] == '\0' || value == NULL)
00943 {
00944 return FALSE;
00945 }
00946
00947 LLQuaternion quat;
00948 S32 count = sscanf( buf, "%f %f %f %f", quat.mQ + 0, quat.mQ + 1, quat.mQ + 2, quat.mQ + 3 );
00949 if( 4 == count )
00950 {
00951 value->set( quat );
00952 return TRUE;
00953 }
00954
00955 return FALSE;
00956 }
00957
00958
00959