00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef CMatrix3dH
00023 #define CMatrix3dH
00024
00025 #include "extras/CGlobals.h"
00026 #include "math/CConstants.h"
00027 #include "math/CVector3d.h"
00028
00029
00030
00038
00039
00040
00050
00051 struct cMatrix3d
00052 {
00053 public:
00054
00055
00057 double m[3][3];
00058
00059
00060
00064
00065 cMatrix3d(){};
00066
00067
00068
00075
00076 inline void set(const double& a_value)
00077 {
00078 m[0][0] = a_value; m[0][1] = a_value; m[0][2] = a_value;
00079 m[1][0] = a_value; m[1][1] = a_value; m[1][2] = a_value;
00080 m[2][0] = a_value; m[2][1] = a_value; m[2][2] = a_value;
00081 }
00082
00083
00084
00098
00099 inline void set(const double& a_m00, const double& a_m01, const double& a_m02,
00100 const double& a_m10, const double& a_m11, const double& a_m12,
00101 const double& a_m20, const double& a_m21, const double& a_m22)
00102 {
00103 m[0][0] = a_m00; m[0][1] = a_m01; m[0][2] = a_m02;
00104 m[1][0] = a_m10; m[1][1] = a_m11; m[1][2] = a_m12;
00105 m[2][0] = a_m20; m[2][1] = a_m21; m[2][2] = a_m22;
00106 }
00107
00108
00109
00118
00119 inline void setCol(const cVector3d& a_vectCol0, const cVector3d& a_vectCol1,
00120 const cVector3d& a_vectCol2)
00121 {
00122 m[0][0] = a_vectCol0.x; m[0][1] = a_vectCol1.x; m[0][2] = a_vectCol2.x;
00123 m[1][0] = a_vectCol0.y; m[1][1] = a_vectCol1.y; m[1][2] = a_vectCol2.y;
00124 m[2][0] = a_vectCol0.z; m[2][1] = a_vectCol1.z; m[2][2] = a_vectCol2.z;
00125 }
00126
00127
00128
00134
00135 inline void setCol0(const cVector3d& a_vectCol)
00136 {
00137 m[0][0] = a_vectCol.x; m[1][0] = a_vectCol.y; m[2][0] = a_vectCol.z;
00138 }
00139
00140
00141
00147
00148 inline void setCol1(const cVector3d& a_vectCol)
00149 {
00150 m[0][1] = a_vectCol.x; m[1][1] = a_vectCol.y; m[2][1] = a_vectCol.z;
00151 }
00152
00153
00154
00160
00161 inline void setCol2(const cVector3d& a_vectCol)
00162 {
00163 m[0][2] = a_vectCol.x; m[1][2] = a_vectCol.y; m[2][2] = a_vectCol.z;
00164 }
00165
00166
00167
00173
00174 inline cVector3d getCol0() const
00175 {
00176 cVector3d result;
00177 result.x = m[0][0]; result.y = m[1][0]; result.z = m[2][0];
00178 return (result);
00179 }
00180
00181
00182
00188
00189 inline cVector3d getCol1() const
00190 {
00191 cVector3d result;
00192 result.x = m[0][1]; result.y = m[1][1]; result.z = m[2][1];
00193 return (result);
00194 }
00195
00196
00197
00203
00204 inline cVector3d getCol2() const
00205 {
00206 cVector3d result;
00207 result.x = m[0][2]; result.y = m[1][2]; result.z = m[2][2];
00208 return (result);
00209 }
00210
00211
00212
00219
00220 inline cVector3d getRow(const unsigned int& index) const
00221 {
00222 cVector3d result;
00223 result.x = m[index][0]; result.y = m[index][1]; result.z = m[index][2];
00224 return (result);
00225 }
00226
00227
00228
00235
00236 inline cVector3d operator[](const unsigned int& index) const
00237 {
00238 return getRow(index);
00239 }
00240
00241
00242
00246
00247 inline void operator/= (const double& a_val)
00248 {
00249 m[0][0] /= a_val; m[0][1] /= a_val; m[0][2] /= a_val;
00250 m[1][0] /= a_val; m[1][1] /= a_val; m[1][2] /= a_val;
00251 m[2][0] /= a_val; m[2][1] /= a_val; m[2][2] /= a_val;
00252 }
00253
00254
00255
00259
00260 inline void operator*= (const double& a_val)
00261 {
00262 m[0][0] *= a_val; m[0][1] *= a_val; m[0][2] *= a_val;
00263 m[1][0] *= a_val; m[1][1] *= a_val; m[1][2] *= a_val;
00264 m[2][0] *= a_val; m[2][1] *= a_val; m[2][2] *= a_val;
00265 }
00266
00267
00268
00272
00273 inline cVector3d operator* (const cVector3d& a_val)
00274 {
00275 cVector3d result;
00276 mulr(a_val,result);
00277 return result;
00278 }
00279
00280
00281
00285
00286 inline cMatrix3d operator* (const cMatrix3d& a_val)
00287 {
00288 cMatrix3d result;
00289 mulr(a_val,result);
00290 return result;
00291 }
00292
00293
00294
00298
00299 inline void operator*= (const cMatrix3d& a_val)
00300 {
00301 mul(a_val);
00302 }
00303
00304
00305
00309
00310 inline void operator+= (const cMatrix3d& a_input)
00311 {
00312 m[0][0] += a_input.m[0][0];
00313 m[0][1] += a_input.m[0][1];
00314 m[0][2] += a_input.m[0][2];
00315
00316 m[1][0] += a_input.m[1][0];
00317 m[1][1] += a_input.m[1][1];
00318 m[1][2] += a_input.m[1][2];
00319
00320 m[2][0] += a_input.m[2][0];
00321 m[2][1] += a_input.m[2][1];
00322 m[2][2] += a_input.m[2][2];
00323 }
00324
00325
00326
00330
00331 inline void operator-= (const cMatrix3d& a_input)
00332 {
00333 m[0][0] -= a_input.m[0][0];
00334 m[0][1] -= a_input.m[0][1];
00335 m[0][2] -= a_input.m[0][2];
00336
00337 m[1][0] -= a_input.m[1][0];
00338 m[1][1] -= a_input.m[1][1];
00339 m[1][2] -= a_input.m[1][2];
00340
00341 m[2][0] -= a_input.m[2][0];
00342 m[2][1] -= a_input.m[2][1];
00343 m[2][2] -= a_input.m[2][2];
00344 }
00345
00346
00347
00353
00354 inline void copyto(cMatrix3d& a_destination) const
00355 {
00356 a_destination.m[0][0] = m[0][0]; a_destination.m[0][1] = m[0][1]; a_destination.m[0][2] = m[0][2];
00357 a_destination.m[1][0] = m[1][0]; a_destination.m[1][1] = m[1][1]; a_destination.m[1][2] = m[1][2];
00358 a_destination.m[2][0] = m[2][0]; a_destination.m[2][1] = m[2][1]; a_destination.m[2][2] = m[2][2];
00359 }
00360
00361
00362
00369
00370 inline void copyfrom(const cMatrix3d& a_source)
00371 {
00372 m[0][0] = a_source.m[0][0]; m[0][1] = a_source.m[0][1]; m[0][2] = a_source.m[0][2];
00373 m[1][0] = a_source.m[1][0]; m[1][1] = a_source.m[1][1]; m[1][2] = a_source.m[1][2];
00374 m[2][0] = a_source.m[2][0]; m[2][1] = a_source.m[2][1]; m[2][2] = a_source.m[2][2];
00375 }
00376
00377
00378
00382
00383 inline void identity()
00384 {
00385 m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0;
00386 m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0;
00387 m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0;
00388 }
00389
00390
00391
00398
00399 inline void mul(const cMatrix3d& a_matrix)
00400 {
00401
00402 double m00 = m[0][0] * a_matrix.m[0][0] + m[0][1] * a_matrix.m[1][0] + m[0][2] * a_matrix.m[2][0];
00403 double m01 = m[0][0] * a_matrix.m[0][1] + m[0][1] * a_matrix.m[1][1] + m[0][2] * a_matrix.m[2][1];
00404 double m02 = m[0][0] * a_matrix.m[0][2] + m[0][1] * a_matrix.m[1][2] + m[0][2] * a_matrix.m[2][2];
00405 double m10 = m[1][0] * a_matrix.m[0][0] + m[1][1] * a_matrix.m[1][0] + m[1][2] * a_matrix.m[2][0];
00406 double m11 = m[1][0] * a_matrix.m[0][1] + m[1][1] * a_matrix.m[1][1] + m[1][2] * a_matrix.m[2][1];
00407 double m12 = m[1][0] * a_matrix.m[0][2] + m[1][1] * a_matrix.m[1][2] + m[1][2] * a_matrix.m[2][2];
00408 double m20 = m[2][0] * a_matrix.m[0][0] + m[2][1] * a_matrix.m[1][0] + m[2][2] * a_matrix.m[2][0];
00409 double m21 = m[2][0] * a_matrix.m[0][1] + m[2][1] * a_matrix.m[1][1] + m[2][2] * a_matrix.m[2][1];
00410 double m22 = m[2][0] * a_matrix.m[0][2] + m[2][1] * a_matrix.m[1][2] + m[2][2] * a_matrix.m[2][2];
00411
00412
00413 m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
00414 m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
00415 m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
00416 }
00417
00418
00419
00428
00429 inline void mulr(const cMatrix3d& a_matrix, cMatrix3d& a_result) const
00430 {
00431
00432 a_result.m[0][0] = m[0][0] * a_matrix.m[0][0] + m[0][1] * a_matrix.m[1][0] + m[0][2] * a_matrix.m[2][0];
00433 a_result.m[0][1] = m[0][0] * a_matrix.m[0][1] + m[0][1] * a_matrix.m[1][1] + m[0][2] * a_matrix.m[2][1];
00434 a_result.m[0][2] = m[0][0] * a_matrix.m[0][2] + m[0][1] * a_matrix.m[1][2] + m[0][2] * a_matrix.m[2][2];
00435 a_result.m[1][0] = m[1][0] * a_matrix.m[0][0] + m[1][1] * a_matrix.m[1][0] + m[1][2] * a_matrix.m[2][0];
00436 a_result.m[1][1] = m[1][0] * a_matrix.m[0][1] + m[1][1] * a_matrix.m[1][1] + m[1][2] * a_matrix.m[2][1];
00437 a_result.m[1][2] = m[1][0] * a_matrix.m[0][2] + m[1][1] * a_matrix.m[1][2] + m[1][2] * a_matrix.m[2][2];
00438 a_result.m[2][0] = m[2][0] * a_matrix.m[0][0] + m[2][1] * a_matrix.m[1][0] + m[2][2] * a_matrix.m[2][0];
00439 a_result.m[2][1] = m[2][0] * a_matrix.m[0][1] + m[2][1] * a_matrix.m[1][1] + m[2][2] * a_matrix.m[2][1];
00440 a_result.m[2][2] = m[2][0] * a_matrix.m[0][2] + m[2][1] * a_matrix.m[1][2] + m[2][2] * a_matrix.m[2][2];
00441 }
00442
00443
00444
00453
00454 inline void mul(cVector3d& a_vector) const
00455 {
00456
00457 double x = m[0][0] * a_vector.x + m[0][1] * a_vector.y + m[0][2] * a_vector.z;
00458 double y = m[1][0] * a_vector.x + m[1][1] * a_vector.y + m[1][2] * a_vector.z;
00459 double z = m[2][0] * a_vector.x + m[2][1] * a_vector.y + m[2][2] * a_vector.z;
00460
00461
00462 a_vector.x = x;
00463 a_vector.y = y;
00464 a_vector.z = z;
00465 }
00466
00467
00468
00477
00478 inline void mulr(const cVector3d& a_vector, cVector3d& a_result) const
00479 {
00480
00481 a_result.x = m[0][0] * a_vector.x + m[0][1] * a_vector.y + m[0][2] * a_vector.z;
00482 a_result.y = m[1][0] * a_vector.x + m[1][1] * a_vector.y + m[1][2] * a_vector.z;
00483 a_result.z = m[2][0] * a_vector.x + m[2][1] * a_vector.y + m[2][2] * a_vector.z;
00484 }
00485
00486
00487
00493
00494 inline double det() const
00495 {
00496 return (+ m[0][0] * m[1][1] * m[2][2]
00497 + m[0][1] * m[1][2] * m[2][0]
00498 + m[0][2] * m[1][0] * m[2][1]
00499 - m[2][0] * m[1][1] * m[0][2]
00500 - m[2][1] * m[1][2] * m[0][0]
00501 - m[2][2] * m[1][0] * m[0][1]);
00502 }
00503
00504
00505
00510
00511 inline void trans()
00512 {
00513 double t;
00514
00515 t = m[0][1]; m[0][1] = m[1][0]; m[1][0] = t;
00516 t = m[0][2]; m[0][2] = m[2][0]; m[2][0] = t;
00517 t = m[1][2]; m[1][2] = m[2][1]; m[2][1] = t;
00518 }
00519
00520
00521
00528
00529 inline void transr(cMatrix3d& a_result) const
00530 {
00531 a_result.m[0][0] = m[0][0];
00532 a_result.m[0][1] = m[1][0];
00533 a_result.m[0][2] = m[2][0];
00534
00535 a_result.m[1][0] = m[0][1];
00536 a_result.m[1][1] = m[1][1];
00537 a_result.m[1][2] = m[2][1];
00538
00539 a_result.m[2][0] = m[0][2];
00540 a_result.m[2][1] = m[1][2];
00541 a_result.m[2][2] = m[2][2];
00542 }
00543
00544
00545
00553
00554 bool invert()
00555 {
00556
00557 double det = (+ m[0][0] * m[1][1] * m[2][2]
00558 + m[0][1] * m[1][2] * m[2][0]
00559 + m[0][2] * m[1][0] * m[2][1]
00560 - m[2][0] * m[1][1] * m[0][2]
00561 - m[2][1] * m[1][2] * m[0][0]
00562 - m[2][2] * m[1][0] * m[0][1]);
00563
00564
00565 if ((det < CHAI_TINY) && (det > -CHAI_TINY))
00566 {
00567
00568 return (false);
00569 }
00570 else
00571 {
00572
00573 double m00 = (m[1][1] * m[2][2] - m[2][1]*m[1][2]) / det;
00574 double m01 = -(m[0][1] * m[2][2] - m[2][1]*m[0][2]) / det;
00575 double m02 = (m[0][1] * m[1][2] - m[1][1]*m[0][2]) / det;
00576
00577 double m10 = -(m[1][0] * m[2][2] - m[2][0]*m[1][2]) / det;
00578 double m11 = (m[0][0] * m[2][2] - m[2][0]*m[0][2]) / det;
00579 double m12 = -(m[0][0] * m[1][2] - m[1][0]*m[0][2]) / det;
00580
00581 double m20 = (m[1][0] * m[2][1] - m[2][0]*m[1][1]) / det;
00582 double m21 = -(m[0][0] * m[2][1] - m[2][0]*m[0][1]) / det;
00583 double m22 = (m[0][0] * m[1][1] - m[1][0]*m[0][1]) / det;
00584
00585
00586 m[0][0] = m00; m[0][1] = m01; m[0][2] = m02;
00587 m[1][0] = m10; m[1][1] = m11; m[1][2] = m12;
00588 m[2][0] = m20; m[2][1] = m21; m[2][2] = m22;
00589
00590
00591 return (true);
00592 }
00593 }
00594
00595
00596
00605
00606 cMatrix3d inv(bool* a_result=0) const
00607 {
00608 cMatrix3d result;
00609 bool status = invertr(result);
00610 if (a_result) *a_result = status;
00611 return result;
00612 }
00613
00614
00615
00625
00626 bool invertr(cMatrix3d& a_result) const
00627 {
00628
00629 double det = (+ m[0][0] * m[1][1] * m[2][2]
00630 + m[0][1] * m[1][2] * m[2][0]
00631 + m[0][2] * m[1][0] * m[2][1]
00632 - m[2][0] * m[1][1] * m[0][2]
00633 - m[2][1] * m[1][2] * m[0][0]
00634 - m[2][2] * m[1][0] * m[0][1]);
00635
00636
00637 if ((det < CHAI_TINY) && (det > -CHAI_TINY))
00638 {
00639
00640 return (false);
00641 }
00642 else
00643 {
00644
00645 a_result.m[0][0] = (m[1][1] * m[2][2] - m[2][1]*m[1][2]) / det;
00646 a_result.m[0][1] = -(m[0][1] * m[2][2] - m[2][1]*m[0][2]) / det;
00647 a_result.m[0][2] = (m[0][1] * m[1][2] - m[1][1]*m[0][2]) / det;
00648
00649 a_result.m[1][0] = -(m[1][0] * m[2][2] - m[2][0]*m[1][2]) / det;
00650 a_result.m[1][1] = (m[0][0] * m[2][2] - m[2][0]*m[0][2]) / det;
00651 a_result.m[1][2] = -(m[0][0] * m[1][2] - m[1][0]*m[0][2]) / det;
00652
00653 a_result.m[2][0] = (m[1][0] * m[2][1] - m[2][0]*m[1][1]) / det;
00654 a_result.m[2][1] = -(m[0][0] * m[2][1] - m[2][0]*m[0][1]) / det;
00655 a_result.m[2][2] = (m[0][0] * m[1][1] - m[1][0]*m[0][1]) / det;
00656
00657
00658 return (true);
00659 }
00660 }
00661
00662
00663
00674
00675 inline bool set(const cVector3d& a_axis, const double& a_angleRad)
00676 {
00677
00678 double length = a_axis.length();
00679
00680
00681 if (length < CHAI_TINY)
00682 {
00683
00684 return (false);
00685 }
00686
00687
00688 double x = a_axis.x / length;
00689 double y = a_axis.y / length;
00690 double z = a_axis.z / length;
00691
00692
00693 double c = cos(a_angleRad);
00694 double s = sin(a_angleRad);
00695 double v = 1-c;
00696
00697 m[0][0] = x*x*v+c; m[0][1] = x*y*v-z*s; m[0][2] = x*z*v+y*s;
00698 m[1][0] = x*y*v+z*s; m[1][1] = y*y*v+c; m[1][2] = y*z*v-x*s;
00699 m[2][0] = x*z*v-y*s; m[2][1] = y*z*v+x*s; m[2][2] = z*z*v+c;
00700
00701
00702 return (true);
00703 }
00704
00705
00706
00714
00715 inline bool rotate(const cVector3d& a_axis, const double& a_angleRad)
00716 {
00717
00718 double length = a_axis.length();
00719
00720
00721 if (length < CHAI_TINY)
00722 {
00723
00724 return (false);
00725 }
00726
00727
00728 double x = a_axis.x / length;
00729 double y = a_axis.y / length;
00730 double z = a_axis.z / length;
00731
00732
00733 double c = cos(a_angleRad);
00734 double s = sin(a_angleRad);
00735 double v = 1-c;
00736
00737 double m00 = x*x*v+c; double m01 = x*y*v-z*s; double m02 = x*z*v+y*s;
00738 double m10 = x*y*v+z*s; double m11 = y*y*v+c; double m12 = y*z*v-x*s;
00739 double m20 = x*z*v-y*s; double m21 = y*z*v+x*s; double m22 = z*z*v+c;
00740
00741
00742 double tm00 = m00 * m[0][0] + m01 * m[1][0] + m02 * m[2][0];
00743 double tm01 = m00 * m[0][1] + m01 * m[1][1] + m02 * m[2][1];
00744 double tm02 = m00 * m[0][2] + m01 * m[1][2] + m02 * m[2][2];
00745 double tm10 = m10 * m[0][0] + m11 * m[1][0] + m12 * m[2][0];
00746 double tm11 = m10 * m[0][1] + m11 * m[1][1] + m12 * m[2][1];
00747 double tm12 = m10 * m[0][2] + m11 * m[1][2] + m12 * m[2][2];
00748 double tm20 = m20 * m[0][0] + m21 * m[1][0] + m22 * m[2][0];
00749 double tm21 = m20 * m[0][1] + m21 * m[1][1] + m22 * m[2][1];
00750 double tm22 = m20 * m[0][2] + m21 * m[1][2] + m22 * m[2][2];
00751
00752
00753 m[0][0] = tm00; m[0][1] = tm01; m[0][2] = tm02;
00754 m[1][0] = tm10; m[1][1] = tm11; m[1][2] = tm12;
00755 m[2][0] = tm20; m[2][1] = tm21; m[2][2] = tm22;
00756
00757
00758 return (true);
00759 }
00760
00761
00762
00772
00773 inline bool rotater(const cVector3d& a_axis, const double& a_angleRad, cMatrix3d& a_result) const
00774 {
00775
00776 double length = a_axis.length();
00777
00778
00779 if (length < CHAI_TINY)
00780 {
00781
00782 return (false);
00783 }
00784
00785
00786 double x = a_axis.x / length;
00787 double y = a_axis.y / length;
00788 double z = a_axis.z / length;
00789
00790
00791 double c = cos(a_angleRad);
00792 double s = sin(a_angleRad);
00793 double v = 1-c;
00794
00795 double m00 = x*x*v+c; double m01 = x*y*v-z*s; double m02 = x*z*v+y*s;
00796 double m10 = x*y*v+z*s; double m11 = y*y*v+c; double m12 = y*z*v-x*s;
00797 double m20 = x*z*v-y*s; double m21 = y*z*v+x*s; double m22 = z*z*v+c;
00798
00799
00800 a_result.m[0][0] = m00 * m[0][0] + m01 * m[1][0] + m02 * m[2][0];
00801 a_result.m[0][1] = m00 * m[0][1] + m01 * m[1][1] + m02 * m[2][1];
00802 a_result.m[0][2] = m00 * m[0][2] + m01 * m[1][2] + m02 * m[2][2];
00803 a_result.m[1][0] = m10 * m[0][0] + m11 * m[1][0] + m12 * m[2][0];
00804 a_result.m[1][1] = m10 * m[0][1] + m11 * m[1][1] + m12 * m[2][1];
00805 a_result.m[1][2] = m10 * m[0][2] + m11 * m[1][2] + m12 * m[2][2];
00806 a_result.m[2][0] = m20 * m[0][0] + m21 * m[1][0] + m22 * m[2][0];
00807 a_result.m[2][1] = m20 * m[0][1] + m21 * m[1][1] + m22 * m[2][1];
00808 a_result.m[2][2] = m20 * m[0][2] + m21 * m[1][2] + m22 * m[2][2];
00809
00810
00811 return (true);
00812 }
00813
00814
00815
00822
00823 inline void str(string& a_string, const unsigned int a_precision=2) const
00824 {
00825 a_string.append("[ ");
00826 for (int i=0; i<3; i++)
00827 {
00828 a_string.append("( ");
00829 for (int j=0; j<3; j++)
00830 {
00831 cStr(a_string, m[j][i], a_precision);
00832 if (j<2)
00833 {
00834 a_string.append(", ");
00835 }
00836 }
00837 a_string.append(" ) ");
00838 }
00839 a_string.append("]");
00840 }
00841
00842
00843
00849
00850 inline string str(const unsigned int a_precision=2) const
00851 {
00852 string a_string;
00853 str(a_string,a_precision);
00854 return a_string;
00855 }
00856
00857
00858
00864
00865 inline void print(const unsigned int a_precision=2) const
00866 {
00867 string s;
00868 str(s,a_precision);
00869 CHAI_DEBUG_PRINT("%s\n",s.c_str());
00870 }
00871
00872
00873
00881
00882 inline bool equals(cMatrix3d& a_matrix) const
00883 {
00884 for(int i=0; i<3; i++)
00885 {
00886 for(int j=0; j<3; j++)
00887 {
00888 if (a_matrix.m[i][j] != m[i][j]) return (false);
00889 }
00890 }
00891 return (true);
00892 }
00893
00894
00895
00903
00904 bool toAngleAxis(double& a_angle, cVector3d& a_axis)
00905 {
00906 double const epsilon = 0.01;
00907 if ( (m[0][1]-m[1][0] < epsilon) && (m[0][1]-m[1][0] > -epsilon) &&
00908 (m[0][2]-m[2][0] < epsilon) && (m[0][2]-m[2][0] > -epsilon) &&
00909 (m[1][2]-m[2][1] < epsilon) && (m[1][2]-m[2][1] > -epsilon) ) {
00910
00911 if ((m[0][1]+m[1][0] < epsilon) && (m[0][1]+m[1][0] > -epsilon) &&
00912 (m[0][2]+m[2][0] < epsilon) && (m[0][2]+m[2][0] > -epsilon) &&
00913 (m[1][2]+m[2][1] < epsilon) && (m[1][2]+m[2][1] > -epsilon) ) {
00914
00915 a_angle = 0;
00916
00917 a_axis[0] = 1.0;
00918 a_axis[1] = 0.0;
00919 a_axis[2] = 0.0;
00920 } else {
00921
00922 a_angle = CHAI_PI;
00923
00924 a_axis[0] = (m[0][0]+1.0)/2.0;
00925 if (a_axis[0] > 0) {
00926 a_axis[0] = sqrt(a_axis[0]);
00927 } else {
00928
00929 return false;
00930 }
00931 a_axis[1] = (m[1][1]+1.0)/2.0;
00932 if (a_axis[1] > 0) {
00933 a_axis[1] = sqrt(a_axis[1]);
00934 } else {
00935
00936 return false;
00937 }
00938 a_axis[2] = (m[2][2]+1.0)/2.0;
00939 if (a_axis[2] > 0) {
00940 a_axis[2] = sqrt(a_axis[2]);
00941 } else {
00942
00943 return false;
00944 }
00945 bool xIsZero = (a_axis[0] < epsilon && a_axis[0] > -epsilon);
00946 bool yIsZero = (a_axis[1] < epsilon && a_axis[1] > -epsilon);
00947 bool zIsZero = (a_axis[2] < epsilon && a_axis[2] > -epsilon);
00948 bool xyIsPositive = (m[0][1] > 0.0);
00949 bool xzIsPositive = (m[0][2] > 0.0);
00950 bool yzIsPositive = (m[1][2] > 0.0);
00951 if (xIsZero && !yIsZero && !zIsZero) {
00952 if (!yzIsPositive) a_axis[1] = -a_axis[1];
00953 } else if (yIsZero && !zIsZero) {
00954 if (!xzIsPositive) a_axis[2] = -a_axis[2];
00955 } else if (zIsZero) {
00956 if (!xyIsPositive) a_axis[0] = -a_axis[0];
00957 }
00958 }
00959 } else {
00960 double sinthetamag = sqrt((m[2][1] - m[1][2])*(m[2][1] - m[1][2])
00961 +(m[0][2] - m[2][0])*(m[0][2] - m[2][0])
00962 +(m[1][0] - m[0][1])*(m[1][0] - m[0][1]));
00963
00964 a_angle = acos(( m[0][0] + m[1][1] + m[2][2] - 1.0)/2.0);
00965 a_axis[0] = (m[2][1] - m[1][2])/sinthetamag;
00966 a_axis[1] = (m[0][2] - m[2][0])/sinthetamag;
00967 a_axis[2] = (m[1][0] - m[0][1])/sinthetamag;
00968
00969 }
00970 return true;
00971 }
00972 };
00973
00974
00975
00976 #endif
00977