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 #ifndef QUATERNION_H_
00026 #define QUATERNION_H_
00027
00028 #include <Core/Primitive/Vector3.h>
00029
00030 namespace Lamp{
00031
00032
00033
00034
00035
00036
00037
00038 class Quaternion{
00039 public:
00040
00041
00042
00043
00044 union{
00045
00046 struct{
00047
00048 float x;
00049
00050 float y;
00051
00052 float z;
00053
00054 float w;
00055 };
00056
00057
00058 float array[4];
00059 };
00060
00061
00062
00063
00064
00065 static const Quaternion zero;
00066
00067
00068 static const Quaternion identity;
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 inline Quaternion(){}
00079
00080
00081
00082
00083
00084
00085
00086
00087 inline Quaternion(
00088 float sourceX, float sourceY, float sourceZ, float sourceW) :
00089 x(sourceX), y(sourceY), z(sourceZ), w(sourceW){
00090 }
00091
00092
00093
00094
00095
00096 inline explicit Quaternion(const float* const source) :
00097 x(source[0]), y(source[1]), z(source[2]), w(source[3]){
00098 }
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 inline void set(float sourceX, float sourceY, float sourceZ, float sourceW){
00111 x = sourceX;
00112 y = sourceY;
00113 z = sourceZ;
00114 w = sourceW;
00115 }
00116
00117
00118
00119
00120
00121 inline void set(const float* const source){
00122 x = source[0];
00123 y = source[1];
00124 z = source[2];
00125 w = source[3];
00126 }
00127
00128
00129
00130
00131 inline void setZero(){
00132 set(0.f, 0.f, 0.f, 0.f);
00133 }
00134
00135
00136
00137
00138 inline void setIdentity(){
00139 set(0.f, 0.f, 0.f, 1.f);
00140 }
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150 inline void setRotationAxis(const Vector3& axis, float radian){
00151 Assert(axis.isUnit());
00152 float halfRadian = radian * 0.5f;
00153 float sin = Math::sin(halfRadian);
00154 x = axis.x * sin;
00155 y = axis.y * sin;
00156 z = axis.z * sin;
00157 w = Math::cos(halfRadian);
00158 }
00159
00160
00161
00162
00163
00164
00165 inline void addRotationAxis(const Vector3& axis, float radian){
00166 Quaternion quaternion;
00167 quaternion.setRotationAxis(axis, radian);
00168 (*this) = quaternion * (*this);
00169 }
00170
00171
00172
00173
00174
00175
00176
00177 inline void getRotationAxis(Vector3* axis, float* radian) const{
00178 Assert(isUnit());
00179 axis->set(array);
00180 float halfRadian = Math::acos(w);
00181 *radian = 2.f * halfRadian;
00182 (*axis) *= 1.f / Math::sin(halfRadian);
00183 }
00184
00185
00186
00187
00188
00189
00190
00191
00192 inline void setRotationXYZ(const Vector3& radian){
00193 float xRadian = radian.x * 0.5f;
00194 float yRadian = radian.y * 0.5f;
00195 float zRadian = radian.z * 0.5f;
00196 float sinX = Math::sin(xRadian);
00197 float cosX = Math::cos(xRadian);
00198 float sinY = Math::sin(yRadian);
00199 float cosY = Math::cos(yRadian);
00200 float sinZ = Math::sin(zRadian);
00201 float cosZ = Math::cos(zRadian);
00202 x = sinX * cosY * cosZ - cosX * sinY * sinZ;
00203 y = cosX * sinY * cosZ + sinX * cosY * sinZ;
00204 z = cosX * cosY * sinZ - sinX * sinY * cosZ;
00205 w = cosX * cosY * cosZ + sinX * sinY * sinZ;
00206 }
00207
00208
00209
00210
00211
00212 inline void addRotationXYZ(const Vector3& radian){
00213 Quaternion quaternion;
00214 quaternion.setRotationXYZ(radian);
00215 (*this) = quaternion * (*this);
00216 }
00217
00218
00219
00220
00221
00222
00223 inline bool getRotationXYZ(Vector3* radian) const{
00224
00225 float x2 = x + x;
00226 float y2 = y + y;
00227 float z2 = z + z;
00228 float xz2 = x * z2;
00229 float wy2 = w * y2;
00230 float temp = -(xz2 - wy2);
00231
00232 if(temp >= 1.f){ temp = 1.f; }
00233 else if(temp <= -1.f){ temp = -1.f; }
00234 float yRadian = Math::asin(temp);
00235 radian->y = yRadian;
00236
00237 float xx2 = x * x2;
00238 float xy2 = x * y2;
00239 float zz2 = z * z2;
00240 float wz2 = w * z2;
00241 if(yRadian < Math::halfPI){
00242 if(yRadian > -Math::halfPI){
00243 float yz2 = y * z2;
00244 float wx2 = w * x2;
00245 float yy2 = y * y2;
00246 radian->x = Math::atan2((yz2 + wx2), (1.f - (xx2 + yy2)));
00247 radian->z = Math::atan2((xy2 + wz2), (1.f - (yy2 + zz2)));
00248 return true;
00249 }else{
00250 radian->x = -Math::atan2((xy2 - wz2), (1.f - (xx2 + zz2)));
00251 radian->z = 0.f;
00252 return false;
00253 }
00254 }else{
00255 radian->x = Math::atan2((xy2 - wz2), (1.f - (xx2 + zz2)));
00256 radian->z = 0.f;
00257 return false;
00258 }
00259 }
00260
00261
00262
00263
00264
00265
00266 inline void setRotationZYX(const Vector3& radian){
00267 float xRadian = radian.x * 0.5f;
00268 float yRadian = radian.y * 0.5f;
00269 float zRadian = radian.z * 0.5f;
00270 float sinX = Math::sin(xRadian);
00271 float cosX = Math::cos(xRadian);
00272 float sinY = Math::sin(yRadian);
00273 float cosY = Math::cos(yRadian);
00274 float sinZ = Math::sin(zRadian);
00275 float cosZ = Math::cos(zRadian);
00276 x = sinX * cosY * cosZ + cosX * sinY * sinZ;
00277 y = cosX * sinY * cosZ - sinX * cosY * sinZ;
00278 z = cosX * cosY * sinZ + sinX * sinY * cosZ;
00279 w = cosX * cosY * cosZ - sinX * sinY * sinZ;
00280 }
00281
00282
00283
00284
00285
00286 inline void addRotationZYX(const Vector3& radian){
00287 Quaternion quaternion;
00288 quaternion.setRotationZYX(radian);
00289 (*this) = quaternion * (*this);
00290 }
00291
00292
00293
00294
00295
00296
00297 inline bool getRotationZYX(Vector3* radian) const{
00298
00299 float x2 = x + x;
00300 float y2 = y + y;
00301 float z2 = z + z;
00302 float xz2 = x * z2;
00303 float wy2 = w * y2;
00304 float temp = (xz2 + wy2);
00305
00306 if(temp >= 1.f){ temp = 1.f; }
00307 else if(temp <= -1.f){ temp = -1.f; }
00308 float yRadian = Math::asin(temp);
00309 radian->y = yRadian;
00310
00311 float xy2 = x * y2;
00312 float wz2 = w * z2;
00313 if(yRadian < Math::halfPI){
00314 if(yRadian > -Math::halfPI){
00315 float yy2 = y * y2;
00316 float zz2 = z * z2;
00317 float yz2 = y * z2;
00318 float wx2 = w * x2;
00319 float xx2 = x * x2;
00320 radian->z = Math::atan2(-(xy2 - wz2), (1.f - (yy2 + zz2)));
00321 radian->x = Math::atan2(-(yz2 - wx2), (1.f - (xx2 + yy2)));
00322 return true;
00323 }else{
00324 radian->z = -Math::atan2(-(xy2 + wz2), (xz2 - wy2));
00325 radian->x = 0.f;
00326 return false;
00327 }
00328 }else{
00329 radian->z = Math::atan2((xy2 + wz2), -(xz2 - wy2));
00330 radian->x = 0.f;
00331 return false;
00332 }
00333 }
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343 inline Quaternion operator +(const Quaternion& addQuaternion) const{
00344 return Quaternion(
00345 x + addQuaternion.x,
00346 y + addQuaternion.y,
00347 z + addQuaternion.z,
00348 w + addQuaternion.w);
00349 }
00350
00351
00352
00353
00354
00355
00356 inline Quaternion operator -(const Quaternion& subQuaternion) const{
00357 return Quaternion(
00358 x - subQuaternion.x,
00359 y - subQuaternion.y,
00360 z - subQuaternion.z,
00361 w - subQuaternion.w);
00362 }
00363
00364
00365
00366
00367
00368
00369 inline Quaternion operator *(const Quaternion& mulQuat) const{
00370
00371
00372 return Quaternion(
00373 y * mulQuat.z - z * mulQuat.y + x * mulQuat.w + w * mulQuat.x,
00374 z * mulQuat.x - x * mulQuat.z + y * mulQuat.w + w * mulQuat.y,
00375 x * mulQuat.y - y * mulQuat.x + z * mulQuat.w + w * mulQuat.z,
00376 w * mulQuat.w - x * mulQuat.x - y * mulQuat.y - z * mulQuat.z);
00377 }
00378
00379
00380
00381
00382
00383
00384 inline Quaternion operator *(float mulValue) const{
00385 return Quaternion(
00386 x * mulValue, y * mulValue, z * mulValue, w * mulValue);
00387 }
00388
00389
00390
00391
00392
00393
00394
00395 inline friend Quaternion operator *(
00396 float mulValue, const Quaternion& mulQuaternion){
00397 return Quaternion(
00398 mulQuaternion.x * mulValue,
00399 mulQuaternion.y * mulValue,
00400 mulQuaternion.z * mulValue,
00401 mulQuaternion.w * mulValue);
00402 }
00403
00404
00405
00406
00407
00408
00409 inline Vector3 operator *(const Vector3& mulVector) const{
00410 Vector3 quaternion(x, y, z);
00411 Vector3 cross1 = quaternion.crossProduct(mulVector);
00412 Vector3 cross2 = quaternion.crossProduct(cross1);
00413 cross1 *= (w * 2.f);
00414 cross1 += mulVector;
00415 cross2 *= 2.f;
00416 cross1 += cross2;
00417 return cross1;
00418 }
00419
00420
00421
00422
00423
00424 inline Quaternion operator +() const{ return *this; }
00425
00426
00427
00428
00429
00430 inline Quaternion operator -() const{ return Quaternion(-x, -y, -z, -w); }
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440 inline Quaternion& operator +=(const Quaternion& addQuaternion){
00441 x += addQuaternion.x;
00442 y += addQuaternion.y;
00443 z += addQuaternion.z;
00444 w += addQuaternion.w;
00445 return *this;
00446 }
00447
00448
00449
00450
00451
00452
00453 inline Quaternion& operator -=(const Quaternion& subQuaternion){
00454 x -= subQuaternion.x;
00455 y -= subQuaternion.y;
00456 z -= subQuaternion.z;
00457 w -= subQuaternion.w;
00458 return *this;
00459 }
00460
00461
00462
00463
00464
00465
00466 inline Quaternion& operator *=(const Quaternion& mulQuat){
00467
00468
00469 set(y * mulQuat.z - z * mulQuat.y + x * mulQuat.w + w * mulQuat.x,
00470 z * mulQuat.x - x * mulQuat.z + y * mulQuat.w + w * mulQuat.y,
00471 x * mulQuat.y - y * mulQuat.x + z * mulQuat.w + w * mulQuat.z,
00472 w * mulQuat.w - x * mulQuat.x - y * mulQuat.y - z * mulQuat.z);
00473 return *this;
00474 }
00475
00476
00477
00478
00479
00480
00481 inline Quaternion& operator *=(float mulValue){
00482 x *= mulValue;
00483 y *= mulValue;
00484 z *= mulValue;
00485 w *= mulValue;
00486 return *this;
00487 }
00488
00489
00490
00491
00492
00493
00494
00495
00496 inline float getLength() const{
00497 return Math::sqrt(x * x + y * y + z * z + w * w);
00498 }
00499
00500
00501
00502
00503
00504 inline bool isZero() const{
00505 return (getLength() <= Math::epsilon);
00506 }
00507
00508
00509
00510
00511
00512 inline bool isUnit() const{
00513 return (Math::abs(getLength() - 1.f) <= Math::epsilon);
00514 }
00515
00516
00517
00518
00519
00520 inline Quaternion& normalize(){
00521 float legnth = getLength();
00522 Assert(legnth >= Math::epsilon);
00523 float inverseLength = 1.f / legnth;
00524 (*this) *= inverseLength;
00525 return *this;
00526 }
00527
00528
00529
00530
00531
00532
00533
00534
00535 inline float getNorm() const{
00536 return x * x + y * y + z * z + w * w;
00537 }
00538
00539
00540
00541
00542
00543 inline Quaternion& conjugate(){
00544 x = -x;
00545 y = -y;
00546 z = -z;
00547 return *this;
00548 }
00549
00550
00551
00552
00553
00554
00555 inline float dotProduct(const Quaternion& dotQuaternion) const{
00556 return x * dotQuaternion.x + y * dotQuaternion.y +
00557 z * dotQuaternion.z + w * dotQuaternion.w;
00558 }
00559
00560
00561
00562
00563
00564 inline Quaternion& invert(){
00565 Assert(!isZero());
00566 float inverseNorm = 1.f / getNorm();
00567 x = -x * inverseNorm;
00568 y = -y * inverseNorm;
00569 z = -z * inverseNorm;
00570 w = w * inverseNorm;
00571 return *this;
00572 }
00573
00574
00575
00576
00577
00578 inline Quaternion& unitInvert(){
00579 Assert(isUnit());
00580 return conjugate();
00581 }
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593 static Quaternion slerp(
00594 const Quaternion& source, const Quaternion& target, float alpha){
00595 Assert(source.isUnit());
00596 Assert(target.isUnit());
00597 float dot = source.dotProduct(target);
00598
00599 if(dot >= 1.f){
00600 dot = 1.f;
00601 }else if(dot <= -1.f){
00602 dot = -1.f;
00603 }
00604 float radian = Math::acos(dot);
00605 if(Math::abs(radian) < Math::epsilon){ return target; }
00606 float inverseSin = 1.f / Math::sin(radian);
00607 float leftScale = Math::sin((1.f - alpha) * radian) * inverseSin;
00608 float rightScale = Math::sin(alpha * radian) * inverseSin;
00609 return source * leftScale + target * rightScale;
00610 }
00611
00612
00613
00614
00615
00616
00617
00618
00619 static Quaternion correctSlerp(
00620 const Quaternion& source, const Quaternion& target, float alpha){
00621 Assert(source.isUnit());
00622 Assert(target.isUnit());
00623 float dot = source.dotProduct(target);
00624
00625 Quaternion correctTarget;
00626 if(dot < 0.f){
00627 correctTarget = -target;
00628 dot = -dot;
00629 }else{
00630 correctTarget = target;
00631 }
00632
00633 if(dot >= 1.f){ dot = 1.f; }
00634 float radian = Math::acos(dot);
00635 if(Math::abs(radian) < Math::epsilon){ return correctTarget; }
00636 float inverseSin = 1.f / Math::sin(radian);
00637 float leftScale = Math::sin((1.f - alpha) * radian) * inverseSin;
00638 float rightScale = Math::sin(alpha * radian) * inverseSin;
00639 return source * leftScale + correctTarget * rightScale;
00640 }
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651 static Quaternion squad(
00652 const Quaternion& source, const Quaternion& sourceHandle,
00653 const Quaternion& targetHandle, const Quaternion& target, float alpha){
00654 Quaternion slerpOriginal = slerp(source, target, alpha);
00655 Quaternion slerpHandle = slerp(sourceHandle, targetHandle, alpha);
00656 return slerp(slerpOriginal, slerpHandle, 2.f * alpha * (1.f - alpha));
00657 }
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668 static Quaternion correctSquad(
00669 const Quaternion& source, const Quaternion& sourceHandle,
00670 const Quaternion& targetHandle, const Quaternion& target, float alpha){
00671 Quaternion slerpOriginal =
00672 correctSlerp(source, target, alpha);
00673 Quaternion slerpHandle =
00674 correctSlerp(sourceHandle, targetHandle, alpha);
00675 return correctSlerp(
00676 slerpOriginal, slerpHandle, 2.f * alpha * (1.f - alpha));
00677 }
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687 inline bool operator ==(const Quaternion& target) const{
00688 return ((x == target.x) && (y == target.y) &&
00689 (z == target.z) && (w == target.w));
00690 }
00691
00692
00693
00694
00695
00696
00697
00698 inline bool epsilonEquals(const Quaternion& target, float epsilon) const{
00699 Assert(epsilon >= 0.f);
00700 return (
00701 (Math::abs(x - target.x) <= epsilon) &&
00702 (Math::abs(y - target.y) <= epsilon) &&
00703 (Math::abs(z - target.z) <= epsilon) &&
00704 (Math::abs(w - target.w) <= epsilon));
00705 }
00706
00707
00708
00709
00710
00711
00712 inline bool operator !=(const Quaternion& target) const{
00713 return ((x != target.x) || (y != target.y) ||
00714 (z != target.z) || (w != target.w));
00715 }
00716
00717
00718
00719
00720
00721
00722
00723 inline bool notEpsilonEquals(const Quaternion& target, float epsilon) const{
00724 Assert(epsilon >= 0.f);
00725 return (
00726 (Math::abs(x - target.x) > epsilon) ||
00727 (Math::abs(y - target.y) > epsilon) ||
00728 (Math::abs(z - target.z) > epsilon) ||
00729 (Math::abs(w - target.w) > epsilon));
00730 }
00731
00732
00733
00734
00735
00736
00737
00738 inline bool rotationEquals(const Quaternion& target) const{
00739 if((*this) == target){ return true; }
00740 if((*this) == -target){ return true; }
00741 return false;
00742 }
00743
00744
00745
00746
00747
00748
00749
00750 inline bool epsilonRotationEquals(
00751 const Quaternion& target, float epsilon) const{
00752 if(epsilonEquals(target, epsilon)){ return true; }
00753 if(epsilonEquals(-target, epsilon)){ return true; }
00754 return false;
00755 }
00756
00757
00758
00759
00760
00761
00762
00763
00764 inline String toString() const{
00765 String returnString;
00766 returnString.format("( < %.8f, %.8f, %.8f > %.8f )", x, y, z, w);
00767 return returnString;
00768 }
00769
00770
00771 private:
00772
00773 };
00774
00775
00776 }
00777 #endif // End of QUATERNION_H_
00778