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 MATRIX43_H_
00026 #define MATRIX43_H_
00027
00028 #include <Core/Primitive/Vector3.h>
00029 #include <Core/Primitive/Quaternion.h>
00030
00031 namespace Lamp{
00032
00033 class Matrix33;
00034 class Matrix44;
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 class Matrix34{
00045 public:
00046
00047
00048
00049
00050 union{
00051
00052 struct{
00053
00054 float m00;
00055
00056 float m01;
00057
00058 float m02;
00059
00060 float m03;
00061
00062 float m10;
00063
00064 float m11;
00065
00066 float m12;
00067
00068 float m13;
00069
00070 float m20;
00071
00072 float m21;
00073
00074 float m22;
00075
00076 float m23;
00077 };
00078
00079
00080 float m[3][4];
00081
00082
00083 float array[12];
00084 };
00085
00086
00087
00088
00089
00090 static const Matrix34 zero;
00091
00092
00093 static const Matrix34 unit;
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103 Matrix34(){}
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120 inline Matrix34(
00121 float i00, float i01, float i02, float i03,
00122 float i10, float i11, float i12, float i13,
00123 float i20, float i21, float i22, float i23) :
00124 m00(i00), m01(i01), m02(i02), m03(i03),
00125 m10(i10), m11(i11), m12(i12), m13(i13),
00126 m20(i20), m21(i21), m22(i22), m23(i23){
00127 }
00128
00129
00130
00131
00132
00133 inline explicit Matrix34(const float* const source) :
00134 m00(source[ 0]), m01(source[ 1]), m02(source[ 2]), m03(source[ 3]),
00135 m10(source[ 4]), m11(source[ 5]), m12(source[ 6]), m13(source[ 7]),
00136 m20(source[ 8]), m21(source[ 9]), m22(source[10]), m23(source[11]){
00137 }
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157 inline void set(
00158 float s00, float s01, float s02, float s03,
00159 float s10, float s11, float s12, float s13,
00160 float s20, float s21, float s22, float s23){
00161 m00 = s00; m01 = s01; m02 = s02; m03 = s03;
00162 m10 = s10; m11 = s11; m12 = s12; m13 = s13;
00163 m20 = s20; m21 = s21; m22 = s22; m23 = s23;
00164 }
00165
00166
00167
00168
00169
00170 inline void set(const float* const source){
00171 m00 = source[ 0]; m01 = source[ 1]; m02 = source[ 2]; m03 = source[ 3];
00172 m10 = source[ 4]; m11 = source[ 5]; m12 = source[ 6]; m13 = source[ 7];
00173 m20 = source[ 8]; m21 = source[ 9]; m22 = source[10]; m23 = source[11];
00174 }
00175
00176
00177
00178
00179 inline void setZero(){
00180 set(0.f, 0.f, 0.f, 0.f,
00181 0.f, 0.f, 0.f, 0.f,
00182 0.f, 0.f, 0.f, 0.f);
00183 }
00184
00185
00186
00187
00188 inline void setUnit(){
00189 set(1.f, 0.f, 0.f, 0.f,
00190 0.f, 1.f, 0.f, 0.f,
00191 0.f, 0.f, 1.f, 0.f);
00192 }
00193
00194
00195
00196
00197
00198 void set(const Matrix33& source);
00199
00200
00201
00202
00203
00204 void set(const Matrix44& source);
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 inline void setScale(float scaleX, float scaleY, float scaleZ){
00216 set(scaleX, 0.f, 0.f, 0.f,
00217 0.f, scaleY, 0.f, 0.f,
00218 0.f, 0.f, scaleZ, 0.f);
00219 }
00220
00221
00222
00223
00224
00225 inline void setScale(const Vector3& scale){
00226 setScale(scale.x, scale.y, scale.z);
00227 }
00228
00229
00230
00231
00232
00233
00234
00235 inline void addScale(float scaleX, float scaleY, float scaleZ){
00236 Matrix34 matrix;
00237 matrix.setScale(scaleX, scaleY, scaleZ);
00238 (*this) = matrix * (*this);
00239 }
00240
00241
00242
00243
00244
00245 inline void addScale(const Vector3& scale){
00246 Matrix34 matrix;
00247 matrix.setScale(scale.x, scale.y, scale.z);
00248 (*this) = matrix * (*this);
00249 }
00250
00251
00252
00253
00254
00255
00256
00257
00258 inline void setRotationX(float radian){
00259 float sin = Math::sin(radian);
00260 float cos = Math::cos(radian);
00261 set( 1.f, 0.f, 0.f, 0.f,
00262 0.f, cos, -sin, 0.f,
00263 0.f, sin, cos, 0.f);
00264 }
00265
00266
00267
00268
00269
00270 inline void addRotationX(float radian){
00271 Matrix34 matrix;
00272 matrix.setRotationX(radian);
00273 (*this) = matrix * (*this);
00274 }
00275
00276
00277
00278
00279
00280
00281 inline void setRotationY(float radian){
00282 float sin = Math::sin(radian);
00283 float cos = Math::cos(radian);
00284 set( cos, 0.f, sin, 0.f,
00285 0.f, 1.f, 0.f, 0.f,
00286 -sin, 0.f, cos, 0.f);
00287 }
00288
00289
00290
00291
00292
00293 inline void addRotationY(float radian){
00294 Matrix34 matrix;
00295 matrix.setRotationY(radian);
00296 (*this) = matrix * (*this);
00297 }
00298
00299
00300
00301
00302
00303
00304 inline void setRotationZ(float radian){
00305 float sin = Math::sin(radian);
00306 float cos = Math::cos(radian);
00307 set( cos, -sin, 0.f, 0.f,
00308 sin, cos, 0.f, 0.f,
00309 0.f, 0.f, 1.f, 0.f);
00310 }
00311
00312
00313
00314
00315
00316 inline void addRotationZ(float radian){
00317 Matrix34 matrix;
00318 matrix.setRotationZ(radian);
00319 (*this) = matrix * (*this);
00320 }
00321
00322
00323
00324
00325
00326
00327
00328 inline void setRotationAxis(const Vector3& axis, float radian){
00329 Assert(axis.isUnit());
00330 float sin = Math::sin(radian);
00331 float cos = Math::cos(radian);
00332 float invCos = 1.f - cos;
00333 float xyInv = axis.x * axis.y * invCos;
00334 float xzInv = axis.x * axis.z * invCos;
00335 float yzInv = axis.y * axis.z * invCos;
00336 float xSin = axis.x * sin;
00337 float ySin = axis.y * sin;
00338 float zSin = axis.z * sin;
00339 set((axis.x * axis.x * invCos + cos), (xyInv - zSin), (xzInv + ySin),
00340 0.f,
00341 (xyInv + zSin), (axis.y * axis.y * invCos + cos), (yzInv - xSin),
00342 0.f,
00343 (xzInv - ySin), (yzInv + xSin), (axis.z * axis.z * invCos + cos),
00344 0.f);
00345 }
00346
00347
00348
00349
00350
00351
00352 inline void addRotationAxis(const Vector3& axis, float radian){
00353 Matrix34 matrix;
00354 matrix.setRotationAxis(axis, radian);
00355 (*this) = matrix * (*this);
00356 }
00357
00358
00359
00360
00361
00362
00363 inline void getRotationAxis(Vector3* axis, float* radian) const{
00364 float radianResult = Math::acos(0.5f * ((m00 + m11 + m22) - 1.f));
00365 *radian = radianResult;
00366 if(radianResult > 0.f){
00367 if(radianResult < Math::PI){
00368 axis->set(m21 - m12, m02 - m20, m10 - m01);
00369 axis->normalize();
00370 }else{
00371 if(m00 >= m11){
00372 if(m00 >= m22){
00373 axis->x = 0.5f * Math::sqrt(m00 - m11 - m22 + 1.f);
00374 float halfInverse = 0.5f / axis->x;
00375 axis->y = halfInverse * m01;
00376 axis->z = halfInverse * m02;
00377 }else{
00378 axis->z = 0.5f * Math::sqrt(m22 - m00 - m11 + 1.f);
00379 float halfInverse = 0.5f / axis->z;
00380 axis->x = halfInverse * m02;
00381 axis->y = halfInverse * m12;
00382 }
00383 }else{
00384 if(m11 >= m22){
00385 axis->y = 0.5f * Math::sqrt(m11 - m00 - m22 + 1.f);
00386 float halfInverse = 0.5f / axis->y;
00387 axis->x = halfInverse * m01;
00388 axis->z = halfInverse * m12;
00389 }else{
00390 axis->z = 0.5f * Math::sqrt(m22 - m00 - m11 + 1.f);
00391 float halfInverse = 0.5f / axis->z;
00392 axis->x = halfInverse * m02;
00393 axis->y = halfInverse * m12;
00394 }
00395 }
00396 }
00397 }else{
00398 axis->set(1.f, 0.f, 0.f);
00399 }
00400 }
00401
00402
00403
00404
00405
00406
00407 inline void setRotationQuaternion(const Quaternion& quaternion){
00408 Assert(quaternion.isUnit());
00409 float x2 = quaternion.x + quaternion.x;
00410 float y2 = quaternion.y + quaternion.y;
00411 float z2 = quaternion.z + quaternion.z;
00412 float xx2 = quaternion.x * x2;
00413 float xy2 = quaternion.x * y2;
00414 float xz2 = quaternion.x * z2;
00415 float yy2 = quaternion.y * y2;
00416 float yz2 = quaternion.y * z2;
00417 float zz2 = quaternion.z * z2;
00418 float wx2 = quaternion.w * x2;
00419 float wy2 = quaternion.w * y2;
00420 float wz2 = quaternion.w * z2;
00421 m00 = 1.f - (yy2 + zz2);
00422 m01 = xy2 - wz2;
00423 m02 = xz2 + wy2;
00424 m03 = 0.f;
00425
00426 m10 = xy2 + wz2;
00427 m11 = 1.f - (xx2 + zz2);
00428 m12 = yz2 - wx2;
00429 m13 = 0.f;
00430
00431 m20 = xz2 - wy2;
00432 m21 = yz2 + wx2;
00433 m22 = 1.f - (xx2 + yy2);
00434 m23 = 0.f;
00435 }
00436
00437
00438
00439
00440
00441 inline void addRotationQuaternion(const Quaternion& quaternion){
00442 Matrix34 matrix;
00443 matrix.setRotationQuaternion(quaternion);
00444 (*this) = matrix * (*this);
00445 }
00446
00447
00448
00449
00450
00451 inline Quaternion getRotationQuaternion() const{
00452 Quaternion result;
00453 float trace = m00 + m11 + m22;
00454 if(trace > 0.f){
00455 float scale = Math::sqrt(trace + 1.f);
00456 result.w = scale * 0.5f;
00457 scale = 0.5f / scale;
00458 result.x = (m21 - m12) * scale;
00459 result.y = (m02 - m20) * scale;
00460 result.z = (m10 - m01) * scale;
00461 }else{
00462 int i = 0;
00463 if(m11 > m00){ i = 1; }
00464 if(m22 > m[i][i]){ i = 2; }
00465 int nextTable[] = { 1, 2, 0 };
00466 int j = nextTable[i];
00467 int k = nextTable[j];
00468 float scale = Math::sqrt(m[i][i] - m[j][j] - m[k][k] + 1.f);
00469 result.array[i] = 0.5f * scale;
00470 scale = 0.5f / scale;
00471 result.w = (m[k][j] - m[j][k]) * scale;
00472 result.array[j] = (m[j][i] + m[i][j]) * scale;
00473 result.array[k] = (m[k][i] + m[i][k]) * scale;
00474 }
00475 return result;
00476 }
00477
00478
00479
00480
00481
00482
00483
00484
00485 inline void setRotationXYZ(const Vector3& radian){
00486 float sinX = Math::sin(radian.x);
00487 float cosX = Math::cos(radian.x);
00488 float sinY = Math::sin(radian.y);
00489 float cosY = Math::cos(radian.y);
00490 float sinZ = Math::sin(radian.z);
00491 float cosZ = Math::cos(radian.z);
00492 m00 = cosY * cosZ;
00493 m01 = sinX * sinY * cosZ - cosX * sinZ;
00494 m02 = cosX * sinY * cosZ + sinX * sinZ;
00495 m03 = 0.f;
00496
00497 m10 = cosY * sinZ;
00498 m11 = sinX * sinY * sinZ + cosX * cosZ;
00499 m12 = cosX * sinY * sinZ - sinX * cosZ;
00500 m13 = 0.f;
00501
00502 m20 = -sinY;
00503 m21 = sinX * cosY;
00504 m22 = cosX * cosY;
00505 m23 = 0.f;
00506 }
00507
00508
00509
00510
00511
00512 inline void addRotationXYZ(const Vector3& radian){
00513 Matrix34 matrix;
00514 matrix.setRotationXYZ(radian);
00515 (*this) = matrix * (*this);
00516 }
00517
00518
00519
00520
00521
00522
00523 inline bool getRotationXYZ(Vector3* radian) const{
00524 float yRadian = Math::asin(-m20);
00525 radian->y = yRadian;
00526 if(yRadian < Math::halfPI){
00527 if(yRadian > -Math::halfPI){
00528 radian->x = Math::atan2(m21, m22);
00529 radian->z = Math::atan2(m10, m00);
00530 return true;
00531 }else{
00532 radian->x = -Math::atan2(m01, m11);
00533 radian->z = 0.f;
00534 return false;
00535 }
00536 }else{
00537 radian->x = Math::atan2(m01, m11);
00538 radian->z = 0.f;
00539 return false;
00540 }
00541 }
00542
00543
00544
00545
00546
00547
00548
00549 inline void setRotationXZY(const Vector3& radian){
00550 setRotationX(radian.x);
00551 addRotationZ(radian.z);
00552 addRotationY(radian.y);
00553 }
00554
00555
00556
00557
00558
00559 inline void addRotationXZY(const Vector3& radian){
00560 Matrix34 matrix;
00561 matrix.setRotationXZY(radian);
00562 (*this) = matrix * (*this);
00563 }
00564
00565
00566
00567
00568
00569
00570 inline bool getRotationXZY(Vector3* radian) const{
00571 float zRadian = Math::asin(m10);
00572 radian->z = zRadian;
00573 if(zRadian < Math::halfPI){
00574 if(zRadian > -Math::halfPI){
00575 radian->x = Math::atan2(-m12, m11);
00576 radian->y = Math::atan2(-m20, m00);
00577 return true;
00578 }else{
00579 radian->x = -Math::atan2(m02, m22);
00580 radian->y = 0.f;
00581 return false;
00582 }
00583 }else{
00584 radian->x = Math::atan2(m02, m22);
00585 radian->y = 0.f;
00586 return false;
00587 }
00588 }
00589
00590
00591
00592
00593
00594
00595
00596 inline void setRotationYXZ(const Vector3& radian){
00597 setRotationY(radian.y);
00598 addRotationX(radian.x);
00599 addRotationZ(radian.z);
00600 }
00601
00602
00603
00604
00605
00606 inline void addRotationYXZ(const Vector3& radian){
00607 Matrix34 matrix;
00608 matrix.setRotationYXZ(radian);
00609 (*this) = matrix * (*this);
00610 }
00611
00612
00613
00614
00615
00616
00617 inline bool getRotationYXZ(Vector3* radian) const{
00618 float xRadian = Math::asin(m21);
00619 radian->x = xRadian;
00620 if(xRadian < Math::halfPI){
00621 if(xRadian > -Math::halfPI){
00622 radian->y = Math::atan2(-m20, m22);
00623 radian->z = Math::atan2(-m01, m11);
00624 return true;
00625 }else{
00626 radian->y = -Math::atan2(-m10, m00);
00627 radian->z = 0.f;
00628 return false;
00629 }
00630 }else{
00631 radian->y = Math::atan2(-m10, m00);
00632 radian->z = 0.f;
00633 return false;
00634 }
00635 }
00636
00637
00638
00639
00640
00641
00642
00643 inline void setRotationYZX(const Vector3& radian){
00644 setRotationY(radian.y);
00645 addRotationZ(radian.z);
00646 addRotationX(radian.x);
00647 }
00648
00649
00650
00651
00652
00653 inline void addRotationYZX(const Vector3& radian){
00654 Matrix34 matrix;
00655 matrix.setRotationYZX(radian);
00656 (*this) = matrix * (*this);
00657 }
00658
00659
00660
00661
00662
00663
00664 inline bool getRotationYZX(Vector3* radian) const{
00665 float zRadian = Math::asin(-m01);
00666 radian->z = zRadian;
00667 if(zRadian < Math::halfPI){
00668 if(zRadian > -Math::halfPI){
00669 radian->y = Math::atan2(m02, m00);
00670 radian->x = Math::atan2(m21, m11);
00671 return true;
00672 }else{
00673 radian->y = -Math::atan2(m12, m22);
00674 radian->x = 0.f;
00675 return false;
00676 }
00677 }else{
00678 radian->y = Math::atan2(m12, m22);
00679 radian->x = 0.f;
00680 return false;
00681 }
00682 }
00683
00684
00685
00686
00687
00688
00689
00690 inline void setRotationZXY(const Vector3& radian){
00691 setRotationZ(radian.z);
00692 addRotationX(radian.x);
00693 addRotationY(radian.y);
00694 }
00695
00696
00697
00698
00699
00700 inline void addRotationZXY(const Vector3& radian){
00701 Matrix34 matrix;
00702 matrix.setRotationZXY(radian);
00703 (*this) = matrix * (*this);
00704 }
00705
00706
00707
00708
00709
00710
00711 inline bool getRotationZXY(Vector3* radian) const{
00712 float xRadian = Math::asin(-m12);
00713 radian->x = xRadian;
00714 if(xRadian < Math::halfPI){
00715 if(xRadian > -Math::halfPI){
00716 radian->z = Math::atan2(m10, m11);
00717 radian->y = Math::atan2(m02, m22);
00718 return true;
00719 }else{
00720 radian->z = -Math::atan2(m20, m00);
00721 radian->y = 0.f;
00722 return false;
00723 }
00724 }else{
00725 radian->z = Math::atan2(m20, m00);
00726 radian->y = 0.f;
00727 return false;
00728 }
00729 }
00730
00731
00732
00733
00734
00735
00736
00737 inline void setRotationZYX(const Vector3& radian){
00738 setRotationZ(radian.z);
00739 addRotationY(radian.y);
00740 addRotationX(radian.x);
00741 }
00742
00743
00744
00745
00746
00747 inline void addRotationZYX(const Vector3& radian){
00748 Matrix34 matrix;
00749 matrix.setRotationZYX(radian);
00750 (*this) = matrix * (*this);
00751 }
00752
00753
00754
00755
00756
00757
00758 inline bool getRotationZYX(Vector3* radian) const{
00759 float yRadian = Math::asin(m02);
00760 radian->y = yRadian;
00761 if(yRadian < Math::halfPI){
00762 if(yRadian > -Math::halfPI){
00763 radian->z = Math::atan2(-m01, m00);
00764 radian->x = Math::atan2(-m12, m22);
00765 return true;
00766 }else{
00767 radian->z = -Math::atan2(-m10, m20);
00768 radian->x = 0.f;
00769 return false;
00770 }
00771 }else{
00772 radian->z = Math::atan2(m10, -m20);
00773 radian->x = 0.f;
00774 return false;
00775 }
00776 }
00777
00778
00779
00780
00781
00782
00783
00784
00785
00786
00787 inline void setTranslation(
00788 float translationX, float translationY, float translationZ){
00789 set(1.f, 0.f, 0.f, translationX,
00790 0.f, 1.f, 0.f, translationY,
00791 0.f, 0.f, 1.f, translationZ);
00792 }
00793
00794
00795
00796
00797
00798 inline void setTranslation(const Vector3& translation){
00799 setTranslation(translation.x, translation.y, translation.z);
00800 }
00801
00802
00803
00804
00805
00806
00807
00808 inline void addTranslation(
00809 float translationX, float translationY, float translationZ){
00810 Matrix34 matrix;
00811 matrix.setTranslation(translationX, translationY, translationZ);
00812 (*this) = matrix * (*this);
00813 }
00814
00815
00816
00817
00818
00819 inline void addTranslation(const Vector3& translation){
00820 Matrix34 matrix;
00821 matrix.setTranslation(translation.x, translation.y, translation.z);
00822 (*this) = matrix * (*this);
00823 }
00824
00825
00826
00827
00828
00829 inline Vector3 getTranslation() const{ return Vector3(m03, m13, m23); }
00830
00831
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841 inline void setTransformationXYZ(
00842 const Vector3& radian, const Vector3& translation){
00843 float sinX = Math::sin(radian.x);
00844 float cosX = Math::cos(radian.x);
00845 float sinY = Math::sin(radian.y);
00846 float cosY = Math::cos(radian.y);
00847 float sinZ = Math::sin(radian.z);
00848 float cosZ = Math::cos(radian.z);
00849 m00 = cosY * cosZ;
00850 m01 = sinX * sinY * cosZ - cosX * sinZ;
00851 m02 = cosX * sinY * cosZ + sinX * sinZ;
00852 m03 = translation.x;
00853
00854 m10 = cosY * sinZ;
00855 m11 = sinX * sinY * sinZ + cosX * cosZ;
00856 m12 = cosX * sinY * sinZ - sinX * cosZ;
00857 m13 = translation.y;
00858
00859 m20 = -sinY;
00860 m21 = sinX * cosY;
00861 m22 = cosX * cosY;
00862 m23 = translation.z;
00863 }
00864
00865
00866
00867
00868
00869
00870
00871
00872 inline void addTransformationXYZ(
00873 const Vector3& radian, const Vector3& translation){
00874 Matrix34 matrix;
00875 matrix.setTransformationXYZ(radian, translation);
00876 (*this) = matrix * (*this);
00877 }
00878
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888 inline void setTransformationXYZ(const Vector3& scale,
00889 const Vector3& radian, const Vector3& translation){
00890 float sinX = Math::sin(radian.x);
00891 float cosX = Math::cos(radian.x);
00892 float sinY = Math::sin(radian.y);
00893 float cosY = Math::cos(radian.y);
00894 float sinZ = Math::sin(radian.z);
00895 float cosZ = Math::cos(radian.z);
00896 m00 = scale.x * (cosY * cosZ);
00897 m01 = scale.y * (sinX * sinY * cosZ - cosX * sinZ);
00898 m02 = scale.z * (cosX * sinY * cosZ + sinX * sinZ);
00899 m03 = translation.x;
00900
00901 m10 = scale.x * (cosY * sinZ);
00902 m11 = scale.y * (sinX * sinY * sinZ + cosX * cosZ);
00903 m12 = scale.z * (cosX * sinY * sinZ - sinX * cosZ);
00904 m13 = translation.y;
00905
00906 m20 = scale.x * (-sinY);
00907 m21 = scale.y * (sinX * cosY);
00908 m22 = scale.z * (cosX * cosY);
00909 m23 = translation.z;
00910 }
00911
00912
00913
00914
00915
00916
00917
00918
00919
00920 inline void addTransformationXYZ(const Vector3& scale,
00921 const Vector3& radian, const Vector3& translation){
00922 Matrix34 matrix;
00923 matrix.setTransformationXYZ(scale, radian, translation);
00924 (*this) = matrix * (*this);
00925 }
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935 inline void setTransformationQuaternion(
00936 const Quaternion& quaternion, const Vector3& translation){
00937 Assert(quaternion.isUnit());
00938 float x2 = quaternion.x + quaternion.x;
00939 float y2 = quaternion.y + quaternion.y;
00940 float z2 = quaternion.z + quaternion.z;
00941 float xx2 = quaternion.x * x2;
00942 float xy2 = quaternion.x * y2;
00943 float xz2 = quaternion.x * z2;
00944 float yy2 = quaternion.y * y2;
00945 float yz2 = quaternion.y * z2;
00946 float zz2 = quaternion.z * z2;
00947 float wx2 = quaternion.w * x2;
00948 float wy2 = quaternion.w * y2;
00949 float wz2 = quaternion.w * z2;
00950 m00 = 1.f - (yy2 + zz2);
00951 m01 = xy2 - wz2;
00952 m02 = xz2 + wy2;
00953 m03 = translation.x;
00954
00955 m10 = xy2 + wz2;
00956 m11 = 1.f - (xx2 + zz2);
00957 m12 = yz2 - wx2;
00958 m13 = translation.y;
00959
00960 m20 = xz2 - wy2;
00961 m21 = yz2 + wx2;
00962 m22 = 1.f - (xx2 + yy2);
00963 m23 = translation.z;
00964
00965 }
00966
00967
00968
00969
00970
00971
00972
00973
00974 inline void addTransformationQuaternion(
00975 const Quaternion& quaternion, const Vector3& translation){
00976 Matrix34 matrix;
00977 matrix.setTransformationQuaternion(quaternion, translation);
00978 (*this) = matrix * (*this);
00979 }
00980
00981
00982
00983
00984
00985
00986
00987
00988
00989
00990 inline void setTransformationQuaternion(const Vector3& scale,
00991 const Quaternion& quaternion, const Vector3& translation){
00992 Assert(quaternion.isUnit());
00993 float x2 = quaternion.x + quaternion.x;
00994 float y2 = quaternion.y + quaternion.y;
00995 float z2 = quaternion.z + quaternion.z;
00996 float xx2 = quaternion.x * x2;
00997 float xy2 = quaternion.x * y2;
00998 float xz2 = quaternion.x * z2;
00999 float yy2 = quaternion.y * y2;
01000 float yz2 = quaternion.y * z2;
01001 float zz2 = quaternion.z * z2;
01002 float wx2 = quaternion.w * x2;
01003 float wy2 = quaternion.w * y2;
01004 float wz2 = quaternion.w * z2;
01005 m00 = scale.x * (1.f - (yy2 + zz2));
01006 m01 = scale.y * (xy2 - wz2);
01007 m02 = scale.z * (xz2 + wy2);
01008 m03 = translation.x;
01009
01010 m10 = scale.x * (xy2 + wz2);
01011 m11 = scale.y * (1.f - (xx2 + zz2));
01012 m12 = scale.z * (yz2 - wx2);
01013 m13 = translation.y;
01014
01015 m20 = scale.x * (xz2 - wy2);
01016 m21 = scale.y * (yz2 + wx2);
01017 m22 = scale.z * (1.f - (xx2 + yy2));
01018 m23 = translation.z;
01019
01020 }
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030 inline void addTransformationQuaternion(const Vector3& scale,
01031 const Quaternion& quaternion, const Vector3& translation){
01032 Matrix34 matrix;
01033 matrix.setTransformationQuaternion(scale, quaternion, translation);
01034 (*this) = matrix * (*this);
01035 }
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047 inline Matrix34 operator *(const Matrix34& mtx) const{
01048 return Matrix34(
01049 (m00 * mtx.m00) + (m01 * mtx.m10) + (m02 * mtx.m20),
01050 (m00 * mtx.m01) + (m01 * mtx.m11) + (m02 * mtx.m21),
01051 (m00 * mtx.m02) + (m01 * mtx.m12) + (m02 * mtx.m22),
01052 (m00 * mtx.m03) + (m01 * mtx.m13) + (m02 * mtx.m23) + m03,
01053 (m10 * mtx.m00) + (m11 * mtx.m10) + (m12 * mtx.m20),
01054 (m10 * mtx.m01) + (m11 * mtx.m11) + (m12 * mtx.m21),
01055 (m10 * mtx.m02) + (m11 * mtx.m12) + (m12 * mtx.m22),
01056 (m10 * mtx.m03) + (m11 * mtx.m13) + (m12 * mtx.m23) + m13,
01057 (m20 * mtx.m00) + (m21 * mtx.m10) + (m22 * mtx.m20),
01058 (m20 * mtx.m01) + (m21 * mtx.m11) + (m22 * mtx.m21),
01059 (m20 * mtx.m02) + (m21 * mtx.m12) + (m22 * mtx.m22),
01060 (m20 * mtx.m03) + (m21 * mtx.m13) + (m22 * mtx.m23) + m23);
01061 }
01062
01063
01064
01065
01066
01067
01068
01069
01070 inline Matrix34& operator *=(Matrix34 mtx){
01071 float old00 = m00;
01072 float old01 = m01;
01073 float old02 = m02;
01074 m00 = (old00 * mtx.m00) + (old01 * mtx.m10) + (old02 * mtx.m20);
01075 m01 = (old00 * mtx.m01) + (old01 * mtx.m11) + (old02 * mtx.m21);
01076 m02 = (old00 * mtx.m02) + (old01 * mtx.m12) + (old02 * mtx.m22);
01077 m03 = (old00 * mtx.m03) + (old01 * mtx.m13) + (old02 * mtx.m23) + m03;
01078 float old10 = m10;
01079 float old11 = m11;
01080 float old12 = m12;
01081 m10 = (old10 * mtx.m00) + (old11 * mtx.m10) + (old12 * mtx.m20);
01082 m11 = (old10 * mtx.m01) + (old11 * mtx.m11) + (old12 * mtx.m21);
01083 m12 = (old10 * mtx.m02) + (old11 * mtx.m12) + (old12 * mtx.m22);
01084 m13 = (old10 * mtx.m03) + (old11 * mtx.m13) + (old12 * mtx.m23) + m13;
01085 float old20 = m20;
01086 float old21 = m21;
01087 float old22 = m22;
01088 m20 = (old20 * mtx.m00) + (old21 * mtx.m10) + (old22 * mtx.m20);
01089 m21 = (old20 * mtx.m01) + (old21 * mtx.m11) + (old22 * mtx.m21);
01090 m22 = (old20 * mtx.m02) + (old21 * mtx.m12) + (old22 * mtx.m22);
01091 m23 = (old20 * mtx.m03) + (old21 * mtx.m13) + (old22 * mtx.m23) + m23;
01092 return *this;
01093 }
01094
01095
01096
01097
01098
01099
01100 inline Vector3 operator *(const Vector3& vector) const{
01101 return Vector3(
01102 vector.x * m00 + vector.y * m01 + vector.z * m02 + m03,
01103 vector.x * m10 + vector.y * m11 + vector.z * m12 + m13,
01104 vector.x * m20 + vector.y * m21 + vector.z * m22 + m23);
01105 }
01106
01107
01108
01109
01110
01111
01112 inline Vector3 multiply33(const Vector3& vector) const{
01113 return Vector3(
01114 vector.x * m00 + vector.y * m01 + vector.z * m02,
01115 vector.x * m10 + vector.y * m11 + vector.z * m12,
01116 vector.x * m20 + vector.y * m21 + vector.z * m22);
01117 }
01118
01119
01120
01121
01122
01123
01124 inline Matrix34 operator *(float value) const{
01125 return Matrix34(
01126 m00 * value, m01 * value, m02 * value, m03 * value,
01127 m10 * value, m11 * value, m12 * value, m13 * value,
01128 m20 * value, m21 * value, m22 * value, m23 * value);
01129 }
01130
01131
01132
01133
01134
01135
01136 inline Matrix34& operator *=(float value){
01137 m00 *= value;
01138 m01 *= value;
01139 m02 *= value;
01140 m03 *= value;
01141 m10 *= value;
01142 m11 *= value;
01143 m12 *= value;
01144 m13 *= value;
01145 m20 *= value;
01146 m21 *= value;
01147 m22 *= value;
01148 m23 *= value;
01149 return *this;
01150 }
01151
01152
01153
01154
01155
01156
01157
01158
01159
01160 inline void transpose(){
01161 float swap;
01162 swap = m01; m01 = m10; m10 = swap;
01163 swap = m02; m02 = m20; m20 = swap;
01164 swap = m12; m12 = m21; m21 = swap;
01165 m03 = m13 = m23 = 0.f;
01166 }
01167
01168
01169
01170
01171
01172 inline float determinant() const{
01173 return
01174 m00 * (m11 * m22 - m12 * m21) -
01175 m01 * (m10 * m22 - m12 * m20) +
01176 m02 * (m10 * m21 - m11 * m20);
01177 }
01178
01179
01180
01181
01182
01183 inline float invert(){
01184 Matrix34 invertMatrix;
01185
01186 invertMatrix.m00 = m11 * m22 - m12 * m21;
01187 invertMatrix.m10 = -(m10 * m22 - m12 * m20);
01188 invertMatrix.m20 = m10 * m21 - m11 * m20;
01189 float determ =
01190 m00 * invertMatrix.m00 +
01191 m01 * invertMatrix.m10 +
01192 m02 * invertMatrix.m20;
01193 Assert(determ > Math::epsilon);
01194
01195 invertMatrix.m01 = -(m01 * m22 - m02 * m21);
01196 invertMatrix.m02 = m01 * m12 - m02 * m11;
01197 invertMatrix.m03 = -(
01198 m01 * (m12 * m23 - m22 * m13) -
01199 m02 * (m11 * m23 - m21 * m13) +
01200 m03 * (m11 * m22 - m21 * m12));
01201
01202 invertMatrix.m11 = m00 * m22 - m02 * m20;
01203 invertMatrix.m12 = -(m00 * m12 - m02 * m10);
01204 invertMatrix.m13 = (
01205 m00 * (m12 * m23 - m22 * m13) -
01206 m02 * (m10 * m23 - m20 * m13) +
01207 m03 * (m10 * m22 - m20 * m12));
01208
01209 invertMatrix.m21 = -(m00 * m21 - m01 * m20);
01210 invertMatrix.m22 = m00 * m11 - m01 * m10;
01211 invertMatrix.m23 = -(
01212 m00 * (m11 * m23 - m21 * m13) -
01213 m01 * (m10 * m23 - m20 * m13) +
01214 m03 * (m10 * m21 - m20 * m11));
01215
01216
01217 float invDeterm = 1.f / determ;
01218 invertMatrix *= invDeterm;
01219 (*this) = invertMatrix;
01220 return determ;
01221 }
01222
01223
01224
01225
01226
01227
01228 inline float invert(Matrix34* invertMatrix) const{
01229 Assert(invertMatrix != NULL);
01230 Assert(invertMatrix != this);
01231
01232 invertMatrix->m00 = m11 * m22 - m12 * m21;
01233 invertMatrix->m10 = -(m10 * m22 - m12 * m20);
01234 invertMatrix->m20 = m10 * m21 - m11 * m20;
01235 float determ =
01236 m00 * invertMatrix->m00 +
01237 m01 * invertMatrix->m10 +
01238 m02 * invertMatrix->m20;
01239 Assert(determ > Math::epsilon);
01240
01241 invertMatrix->m01 = -(m01 * m22 - m02 * m21);
01242 invertMatrix->m02 = m01 * m12 - m02 * m11;
01243 invertMatrix->m03 = -(
01244 m01 * (m12 * m23 - m22 * m13) -
01245 m02 * (m11 * m23 - m21 * m13) +
01246 m03 * (m11 * m22 - m21 * m12));
01247
01248 invertMatrix->m11 = m00 * m22 - m02 * m20;
01249 invertMatrix->m12 = -(m00 * m12 - m02 * m10);
01250 invertMatrix->m13 = (
01251 m00 * (m12 * m23 - m22 * m13) -
01252 m02 * (m10 * m23 - m20 * m13) +
01253 m03 * (m10 * m22 - m20 * m12));
01254
01255 invertMatrix->m21 = -(m00 * m21 - m01 * m20);
01256 invertMatrix->m22 = m00 * m11 - m01 * m10;
01257 invertMatrix->m23 = -(
01258 m00 * (m11 * m23 - m21 * m13) -
01259 m01 * (m10 * m23 - m20 * m13) +
01260 m03 * (m10 * m21 - m20 * m11));
01261
01262
01263 float invDeterm = 1.f / determ;
01264 (*invertMatrix) *= invDeterm;
01265 return determ;
01266 }
01267
01268
01269
01270
01271
01272
01273
01274 inline void invertTransformation(){
01275
01276 float swap;
01277 swap = m01; m01 = m10; m10 = swap;
01278 swap = m02; m02 = m20; m20 = swap;
01279 swap = m12; m12 = m21; m21 = swap;
01280
01281 Vector3 trans(-m03, -m13, -m23);
01282 m03 = m00 * trans.x + m01 * trans.y + m02 * trans.z;
01283 m13 = m10 * trans.x + m11 * trans.y + m12 * trans.z;
01284 m23 = m20 * trans.x + m21 * trans.y + m22 * trans.z;
01285 }
01286
01287
01288
01289
01290
01291
01292
01293 inline void invertTransformation(Matrix34* invertMatrix) const{
01294
01295 invertMatrix->m00 = m00;
01296 invertMatrix->m01 = m10;
01297 invertMatrix->m02 = m20;
01298 invertMatrix->m10 = m01;
01299 invertMatrix->m11 = m11;
01300 invertMatrix->m12 = m21;
01301 invertMatrix->m20 = m02;
01302 invertMatrix->m21 = m12;
01303 invertMatrix->m22 = m22;
01304
01305 Vector3 trans(-m03, -m13, -m23);
01306 invertMatrix->m03 = m00 * trans.x + m10 * trans.y + m20 * trans.z;
01307 invertMatrix->m13 = m01 * trans.x + m11 * trans.y + m21 * trans.z;
01308 invertMatrix->m23 = m02 * trans.x + m12 * trans.y + m22 * trans.z;
01309 }
01310
01311
01312
01313
01314
01315
01316
01317
01318
01319 inline bool operator ==(const Matrix34& target) const{
01320 return (
01321 (m00 == target.m00) && (m01 == target.m01) &&
01322 (m02 == target.m02) && (m03 == target.m03) &&
01323 (m10 == target.m10) && (m11 == target.m11) &&
01324 (m12 == target.m12) && (m13 == target.m13) &&
01325 (m20 == target.m20) && (m21 == target.m21) &&
01326 (m22 == target.m22) && (m23 == target.m23));
01327 }
01328
01329
01330
01331
01332
01333
01334
01335 inline bool epsilonEquals(const Matrix34& target, float epsilon) const{
01336 Assert(epsilon >= 0.f);
01337 return (
01338 (Math::abs(m00 - target.m00) <= epsilon) &&
01339 (Math::abs(m01 - target.m01) <= epsilon) &&
01340 (Math::abs(m02 - target.m02) <= epsilon) &&
01341 (Math::abs(m03 - target.m03) <= epsilon) &&
01342 (Math::abs(m10 - target.m10) <= epsilon) &&
01343 (Math::abs(m11 - target.m11) <= epsilon) &&
01344 (Math::abs(m12 - target.m12) <= epsilon) &&
01345 (Math::abs(m13 - target.m13) <= epsilon) &&
01346 (Math::abs(m20 - target.m20) <= epsilon) &&
01347 (Math::abs(m21 - target.m21) <= epsilon) &&
01348 (Math::abs(m22 - target.m22) <= epsilon) &&
01349 (Math::abs(m23 - target.m23) <= epsilon));
01350 }
01351
01352
01353
01354
01355
01356
01357 inline bool operator !=(const Matrix34& target) const{
01358 return (
01359 (m00 != target.m00) || (m01 != target.m01) ||
01360 (m02 != target.m02) || (m03 != target.m03) ||
01361 (m10 != target.m10) || (m11 != target.m11) ||
01362 (m12 != target.m12) || (m13 != target.m13) ||
01363 (m20 != target.m20) || (m21 != target.m21) ||
01364 (m22 != target.m22) || (m23 != target.m23));
01365 }
01366
01367
01368
01369
01370
01371
01372
01373 inline bool notEpsilonEquals(const Matrix34& target, float epsilon) const{
01374 Assert(epsilon >= 0.f);
01375 return (
01376 (Math::abs(m00 - target.m00) > epsilon) ||
01377 (Math::abs(m01 - target.m01) > epsilon) ||
01378 (Math::abs(m02 - target.m02) > epsilon) ||
01379 (Math::abs(m03 - target.m03) > epsilon) ||
01380 (Math::abs(m10 - target.m10) > epsilon) ||
01381 (Math::abs(m11 - target.m11) > epsilon) ||
01382 (Math::abs(m12 - target.m12) > epsilon) ||
01383 (Math::abs(m13 - target.m13) > epsilon) ||
01384 (Math::abs(m20 - target.m20) > epsilon) ||
01385 (Math::abs(m21 - target.m21) > epsilon) ||
01386 (Math::abs(m22 - target.m22) > epsilon) ||
01387 (Math::abs(m23 - target.m23) > epsilon));
01388 }
01389
01390
01391
01392
01393
01394
01395
01396
01397 inline String toString() const{
01398 String returnString;
01399 returnString.format(
01400 "{\n ( %.8f, %.8f, %.8f, %.8f )\n ( %.8f, %.8f, %.8f, %.8f )\n"
01401 " ( %.8f, %.8f, %.8f, %.8f )\n}",
01402 m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23);
01403 return returnString;
01404 }
01405
01406
01407 private:
01408
01409 };
01410
01411
01412 }
01413 #endif // End of MATRIX43_H_
01414