00001
00032 #ifndef LLQUATERNION_H
00033 #define LLQUATERNION_H
00034
00035 #include "llmath.h"
00036
00037 class LLVector4;
00038 class LLVector3;
00039 class LLVector3d;
00040 class LLMatrix4;
00041 class LLMatrix3;
00042
00043
00044
00045
00046
00047
00048 static const U32 LENGTHOFQUAT = 4;
00049
00050 class LLQuaternion
00051 {
00052 public:
00053 F32 mQ[LENGTHOFQUAT];
00054
00055 static const LLQuaternion DEFAULT;
00056
00057 LLQuaternion();
00058 explicit LLQuaternion(const LLMatrix4 &mat);
00059 explicit LLQuaternion(const LLMatrix3 &mat);
00060 LLQuaternion(F32 x, F32 y, F32 z, F32 w);
00061 LLQuaternion(F32 angle, const LLVector4 &vec);
00062 LLQuaternion(F32 angle, const LLVector3 &vec);
00063 LLQuaternion(const F32 *q);
00064 LLQuaternion(const LLVector3 &x_axis,
00065 const LLVector3 &y_axis,
00066 const LLVector3 &z_axis);
00067
00068 BOOL isIdentity() const;
00069 BOOL isNotIdentity() const;
00070 BOOL isFinite() const;
00071 void quantize16(F32 lower, F32 upper);
00072 void quantize8(F32 lower, F32 upper);
00073 void loadIdentity();
00074
00075 const LLQuaternion& set(F32 x, F32 y, F32 z, F32 w);
00076 const LLQuaternion& set(const LLQuaternion &quat);
00077 const LLQuaternion& set(const F32 *q);
00078 const LLQuaternion& set(const LLMatrix3 &mat);
00079 const LLQuaternion& set(const LLMatrix4 &mat);
00080
00081 const LLQuaternion& setAngleAxis(F32 angle, F32 x, F32 y, F32 z);
00082 const LLQuaternion& setAngleAxis(F32 angle, const LLVector3 &vec);
00083 const LLQuaternion& setAngleAxis(F32 angle, const LLVector4 &vec);
00084 const LLQuaternion& setEulerAngles(F32 roll, F32 pitch, F32 yaw);
00085
00086 const LLQuaternion& setQuatInit(F32 x, F32 y, F32 z, F32 w);
00087 const LLQuaternion& setQuat(const LLQuaternion &quat);
00088 const LLQuaternion& setQuat(const F32 *q);
00089 const LLQuaternion& setQuat(const LLMatrix3 &mat);
00090 const LLQuaternion& setQuat(const LLMatrix4 &mat);
00091 const LLQuaternion& setQuat(F32 angle, F32 x, F32 y, F32 z);
00092 const LLQuaternion& setQuat(F32 angle, const LLVector3 &vec);
00093 const LLQuaternion& setQuat(F32 angle, const LLVector4 &vec);
00094 const LLQuaternion& setQuat(F32 roll, F32 pitch, F32 yaw);
00095
00096 LLMatrix4 getMatrix4(void) const;
00097 LLMatrix3 getMatrix3(void) const;
00098 void getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const;
00099 void getAngleAxis(F32* angle, LLVector3 &vec) const;
00100 void getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const;
00101
00102 F32 normalize();
00103 F32 normQuat();
00104
00105 const LLQuaternion& conjugate(void);
00106 const LLQuaternion& conjQuat(void);
00107
00108
00109 const LLQuaternion& transpose();
00110 const LLQuaternion& transQuat();
00111
00112 void shortestArc(const LLVector3 &a, const LLVector3 &b);
00113 const LLQuaternion& constrain(F32 radians);
00114
00115
00116 friend std::ostream& operator<<(std::ostream &s, const LLQuaternion &a);
00117 friend LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b);
00118 friend LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b);
00119 friend LLQuaternion operator-(const LLQuaternion &a);
00120 friend LLQuaternion operator*(F32 a, const LLQuaternion &q);
00121 friend LLQuaternion operator*(const LLQuaternion &q, F32 b);
00122 friend LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b);
00123 friend LLQuaternion operator~(const LLQuaternion &a);
00124 bool operator==(const LLQuaternion &b) const;
00125 bool operator!=(const LLQuaternion &b) const;
00126
00127 friend const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b);
00128
00129 friend LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot);
00130 friend LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot);
00131 friend LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot);
00132
00133
00134 friend F32 dot(const LLQuaternion &a, const LLQuaternion &b);
00135 friend LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);
00136 friend LLQuaternion lerp(F32 t, const LLQuaternion &q);
00137 friend LLQuaternion slerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);
00138 friend LLQuaternion slerp(F32 t, const LLQuaternion &q);
00139 friend LLQuaternion nlerp(F32 t, const LLQuaternion &p, const LLQuaternion &q);
00140 friend LLQuaternion nlerp(F32 t, const LLQuaternion &q);
00141
00142 LLVector3 packToVector3() const;
00143 void unpackFromVector3(const LLVector3& vec);
00144
00145 enum Order {
00146 XYZ = 0,
00147 YZX = 1,
00148 ZXY = 2,
00149 XZY = 3,
00150 YXZ = 4,
00151 ZYX = 5
00152 };
00153
00154
00155 friend LLQuaternion mayaQ(F32 x, F32 y, F32 z, Order order);
00156
00157
00158 friend const char *OrderToString( const Order order );
00159 friend Order StringToOrder( const char *str );
00160
00161 static BOOL parseQuat(const char* buf, LLQuaternion* value);
00162
00163
00164
00165 };
00166
00167
00168 inline BOOL LLQuaternion::isFinite() const
00169 {
00170 return (llfinite(mQ[VX]) && llfinite(mQ[VY]) && llfinite(mQ[VZ]) && llfinite(mQ[VS]));
00171 }
00172
00173 inline BOOL LLQuaternion::isIdentity() const
00174 {
00175 return
00176 ( mQ[VX] == 0.f ) &&
00177 ( mQ[VY] == 0.f ) &&
00178 ( mQ[VZ] == 0.f ) &&
00179 ( mQ[VS] == 1.f );
00180 }
00181
00182 inline BOOL LLQuaternion::isNotIdentity() const
00183 {
00184 return
00185 ( mQ[VX] != 0.f ) ||
00186 ( mQ[VY] != 0.f ) ||
00187 ( mQ[VZ] != 0.f ) ||
00188 ( mQ[VS] != 1.f );
00189 }
00190
00191
00192
00193 inline LLQuaternion::LLQuaternion(void)
00194 {
00195 mQ[VX] = 0.f;
00196 mQ[VY] = 0.f;
00197 mQ[VZ] = 0.f;
00198 mQ[VS] = 1.f;
00199 }
00200
00201 inline LLQuaternion::LLQuaternion(F32 x, F32 y, F32 z, F32 w)
00202 {
00203 mQ[VX] = x;
00204 mQ[VY] = y;
00205 mQ[VZ] = z;
00206 mQ[VS] = w;
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216 }
00217
00218 inline LLQuaternion::LLQuaternion(const F32 *q)
00219 {
00220 mQ[VX] = q[VX];
00221 mQ[VY] = q[VY];
00222 mQ[VZ] = q[VZ];
00223 mQ[VS] = q[VW];
00224
00225 normalize();
00226
00227
00228
00229
00230
00231
00232 }
00233
00234
00235 inline void LLQuaternion::loadIdentity()
00236 {
00237 mQ[VX] = 0.0f;
00238 mQ[VY] = 0.0f;
00239 mQ[VZ] = 0.0f;
00240 mQ[VW] = 1.0f;
00241 }
00242
00243
00244 inline const LLQuaternion& LLQuaternion::set(F32 x, F32 y, F32 z, F32 w)
00245 {
00246 mQ[VX] = x;
00247 mQ[VY] = y;
00248 mQ[VZ] = z;
00249 mQ[VS] = w;
00250 normalize();
00251 return (*this);
00252 }
00253
00254 inline const LLQuaternion& LLQuaternion::set(const LLQuaternion &quat)
00255 {
00256 mQ[VX] = quat.mQ[VX];
00257 mQ[VY] = quat.mQ[VY];
00258 mQ[VZ] = quat.mQ[VZ];
00259 mQ[VW] = quat.mQ[VW];
00260 normalize();
00261 return (*this);
00262 }
00263
00264 inline const LLQuaternion& LLQuaternion::set(const F32 *q)
00265 {
00266 mQ[VX] = q[VX];
00267 mQ[VY] = q[VY];
00268 mQ[VZ] = q[VZ];
00269 mQ[VS] = q[VW];
00270 normalize();
00271 return (*this);
00272 }
00273
00274
00275
00276 inline const LLQuaternion& LLQuaternion::setQuatInit(F32 x, F32 y, F32 z, F32 w)
00277 {
00278 mQ[VX] = x;
00279 mQ[VY] = y;
00280 mQ[VZ] = z;
00281 mQ[VS] = w;
00282 normalize();
00283 return (*this);
00284 }
00285
00286
00287 inline const LLQuaternion& LLQuaternion::setQuat(const LLQuaternion &quat)
00288 {
00289 mQ[VX] = quat.mQ[VX];
00290 mQ[VY] = quat.mQ[VY];
00291 mQ[VZ] = quat.mQ[VZ];
00292 mQ[VW] = quat.mQ[VW];
00293 normalize();
00294 return (*this);
00295 }
00296
00297
00298 inline const LLQuaternion& LLQuaternion::setQuat(const F32 *q)
00299 {
00300 mQ[VX] = q[VX];
00301 mQ[VY] = q[VY];
00302 mQ[VZ] = q[VZ];
00303 mQ[VS] = q[VW];
00304 normalize();
00305 return (*this);
00306 }
00307
00308
00309
00310
00311 inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
00312 {
00313 F32 cos_a = mQ[VW];
00314 if (cos_a > 1.0f) cos_a = 1.0f;
00315 if (cos_a < -1.0f) cos_a = -1.0f;
00316
00317 F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
00318
00319 if ( fabs( sin_a ) < 0.0005f )
00320 sin_a = 1.0f;
00321 else
00322 sin_a = 1.f/sin_a;
00323
00324 F32 temp_angle = 2.0f * (F32) acos( cos_a );
00325 if (temp_angle > F_PI)
00326 {
00327
00328
00329
00330
00331
00332
00333
00334 *angle = 2.f * F_PI - temp_angle;
00335 *x = - mQ[VX] * sin_a;
00336 *y = - mQ[VY] * sin_a;
00337 *z = - mQ[VZ] * sin_a;
00338 }
00339 else
00340 {
00341 *angle = temp_angle;
00342 *x = mQ[VX] * sin_a;
00343 *y = mQ[VY] * sin_a;
00344 *z = mQ[VZ] * sin_a;
00345 }
00346 }
00347
00348 inline const LLQuaternion& LLQuaternion::conjugate()
00349 {
00350 mQ[VX] *= -1.f;
00351 mQ[VY] *= -1.f;
00352 mQ[VZ] *= -1.f;
00353 return (*this);
00354 }
00355
00356 inline const LLQuaternion& LLQuaternion::conjQuat()
00357 {
00358 mQ[VX] *= -1.f;
00359 mQ[VY] *= -1.f;
00360 mQ[VZ] *= -1.f;
00361 return (*this);
00362 }
00363
00364
00365 inline const LLQuaternion& LLQuaternion::transpose()
00366 {
00367 mQ[VX] *= -1.f;
00368 mQ[VY] *= -1.f;
00369 mQ[VZ] *= -1.f;
00370 return (*this);
00371 }
00372
00373
00374 inline const LLQuaternion& LLQuaternion::transQuat()
00375 {
00376 mQ[VX] *= -1.f;
00377 mQ[VY] *= -1.f;
00378 mQ[VZ] *= -1.f;
00379 return (*this);
00380 }
00381
00382
00383 inline LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b)
00384 {
00385 return LLQuaternion(
00386 a.mQ[VX] + b.mQ[VX],
00387 a.mQ[VY] + b.mQ[VY],
00388 a.mQ[VZ] + b.mQ[VZ],
00389 a.mQ[VW] + b.mQ[VW] );
00390 }
00391
00392
00393 inline LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b)
00394 {
00395 return LLQuaternion(
00396 a.mQ[VX] - b.mQ[VX],
00397 a.mQ[VY] - b.mQ[VY],
00398 a.mQ[VZ] - b.mQ[VZ],
00399 a.mQ[VW] - b.mQ[VW] );
00400 }
00401
00402
00403 inline LLQuaternion operator-(const LLQuaternion &a)
00404 {
00405 return LLQuaternion(
00406 -a.mQ[VX],
00407 -a.mQ[VY],
00408 -a.mQ[VZ],
00409 -a.mQ[VW] );
00410 }
00411
00412
00413 inline LLQuaternion operator*(F32 a, const LLQuaternion &q)
00414 {
00415 return LLQuaternion(
00416 a * q.mQ[VX],
00417 a * q.mQ[VY],
00418 a * q.mQ[VZ],
00419 a * q.mQ[VW] );
00420 }
00421
00422
00423 inline LLQuaternion operator*(const LLQuaternion &q, F32 a)
00424 {
00425 return LLQuaternion(
00426 a * q.mQ[VX],
00427 a * q.mQ[VY],
00428 a * q.mQ[VZ],
00429 a * q.mQ[VW] );
00430 }
00431
00432 inline LLQuaternion operator~(const LLQuaternion &a)
00433 {
00434 LLQuaternion q(a);
00435 q.conjQuat();
00436 return q;
00437 }
00438
00439 inline bool LLQuaternion::operator==(const LLQuaternion &b) const
00440 {
00441 return ( (mQ[VX] == b.mQ[VX])
00442 &&(mQ[VY] == b.mQ[VY])
00443 &&(mQ[VZ] == b.mQ[VZ])
00444 &&(mQ[VS] == b.mQ[VS]));
00445 }
00446
00447 inline bool LLQuaternion::operator!=(const LLQuaternion &b) const
00448 {
00449 return ( (mQ[VX] != b.mQ[VX])
00450 ||(mQ[VY] != b.mQ[VY])
00451 ||(mQ[VZ] != b.mQ[VZ])
00452 ||(mQ[VS] != b.mQ[VS]));
00453 }
00454
00455 inline const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b)
00456 {
00457 #if 1
00458 LLQuaternion q(
00459 b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
00460 b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
00461 b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
00462 b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
00463 );
00464 a = q;
00465 #else
00466 a = a * b;
00467 #endif
00468 return a;
00469 }
00470
00471 inline F32 LLQuaternion::normalize()
00472 {
00473 F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
00474
00475 if (mag > FP_MAG_THRESHOLD)
00476 {
00477 F32 oomag = 1.f/mag;
00478 mQ[VX] *= oomag;
00479 mQ[VY] *= oomag;
00480 mQ[VZ] *= oomag;
00481 mQ[VS] *= oomag;
00482 }
00483 else
00484 {
00485 mQ[VX] = 0.f;
00486 mQ[VY] = 0.f;
00487 mQ[VZ] = 0.f;
00488 mQ[VS] = 1.f;
00489 }
00490
00491 return mag;
00492 }
00493
00494
00495 inline F32 LLQuaternion::normQuat()
00496 {
00497 F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
00498
00499 if (mag > FP_MAG_THRESHOLD)
00500 {
00501 F32 oomag = 1.f/mag;
00502 mQ[VX] *= oomag;
00503 mQ[VY] *= oomag;
00504 mQ[VZ] *= oomag;
00505 mQ[VS] *= oomag;
00506 }
00507 else
00508 {
00509 mQ[VX] = 0.f;
00510 mQ[VY] = 0.f;
00511 mQ[VZ] = 0.f;
00512 mQ[VS] = 1.f;
00513 }
00514
00515 return mag;
00516 }
00517
00518 LLQuaternion::Order StringToOrder( const char *str );
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575 #endif