00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef GENLIB2_ROTMATRIX3_INCLUDED
00022 #define GENLIB2_ROTMATRIX3_INCLUDED
00023
00024 #include <iostream>
00025 #include <cassert>
00026 #include <vector>
00027
00028
00029
00030
00031
00032 #include <cstring>
00033
00034 using namespace std;
00035
00036 #include "rmat3supp.h"
00037
00038
00039
00040
00041 #if defined(__GNUG__)
00042 #define FRIEND_FUNC_TEMPLATE <>
00043 #else
00044 #define FRIEND_FUNC_TEMPLATE
00045 #endif
00046
00047
00048 #define A11 _a[0][0]
00049 #define A12 _a[0][1]
00050 #define A13 _a[0][2]
00051 #define A21 _a[1][0]
00052 #define A22 _a[1][1]
00053 #define A23 _a[1][2]
00054 #define A31 _a[2][0]
00055 #define A32 _a[2][1]
00056 #define A33 _a[2][2]
00057
00058
00059 extern "C" {
00060 double sqrt(double);
00061 double sin(double);
00062 double cos(double);
00063 double asin(double);
00064 double atan2(double, double);
00065 }
00066
00067 namespace GenLib2
00068 {
00069
00070 template <class T>
00071 class RotMatrix3;
00072
00073 template <class T>
00074 istream& operator>>(istream& is, RotMatrix3<T>& M);
00075 template <class T>
00076 ostream& operator<<(ostream& os, const RotMatrix3<T>& M);
00077 template <class T>
00078 RotMatrix3<T> operator*(const RotMatrix3<T>& A, const RotMatrix3<T>& B);
00079
00080
00222 template<class T>
00223 class RotMatrix3
00224 {
00225 public:
00226
00227 #if _MSC_VER == 1400
00228 template<typename T> friend istream& operator >> (istream&, RotMatrix3<T>&);
00229 template<typename T> friend ostream& operator << (ostream&, const RotMatrix3<T>&);
00230 template<typename T> friend RotMatrix3<T> operator * (const RotMatrix3<T>&, const RotMatrix3<T>&);
00231 #else
00232 friend istream& operator>> FRIEND_FUNC_TEMPLATE (istream& is,
00233 RotMatrix3<T>& M);
00234 friend ostream& operator<< FRIEND_FUNC_TEMPLATE (ostream& os,
00235 const RotMatrix3<T>& M);
00236 friend RotMatrix3<T> operator* FRIEND_FUNC_TEMPLATE (const RotMatrix3<T>& A,
00237 const RotMatrix3<T>& B);
00238 #endif
00239
00241 void identity();
00242
00244 RotMatrix3() { identity(); }
00245
00247 RotMatrix3(const RotMatrix3& M);
00248
00250 RotMatrix3& operator=(const RotMatrix3& M);
00251
00253 T& operator ()(int i, int j)
00254 {
00255 assert((i >= 0) && (i < 3));
00256 assert((j >= 0) && (j < 3));
00257 return _a[i][j];
00258 }
00259
00261 const T& operator ()(int i, int j) const
00262 {
00263 assert((i >= 0) && (i < 3));
00264 assert((j >= 0) && (j < 3));
00265 return _a[i][j];
00266 }
00267
00281 void set(char axis, T a);
00282
00290
00291 void set_matrix(const T* v)
00292 {
00293 assert(v != 0);
00294
00295 A11 = v[0]; A12 = v[1]; A13 = v[2];
00296 A21 = v[3]; A22 = v[4]; A23 = v[5];
00297 A31 = v[6]; A32 = v[7]; A33 = v[8];
00298 }
00299
00301 void get_matrix(T* v) const
00302 {
00303 assert(v != 0);
00304
00305 v[0] = A11; v[1] = A12; v[2] = A13;
00306 v[3] = A21; v[4] = A22; v[5] = A23;
00307 v[6] = A31; v[7] = A32; v[8] = A33;
00308 }
00310
00324
00325 void set_angles_opk_fix(T omega, T phi, T kappa)
00326 {
00327 T so = (T)sin((double)omega);
00328 T sp = (T)sin((double)phi);
00329 T sk = (T)sin((double)kappa);
00330
00331 T co = (T)cos((double)omega);
00332 T cp = (T)cos((double)phi);
00333 T ck = (T)cos((double)kappa);
00334
00335 A11 = cp * ck;
00336 A12 = co * sk + so * sp * ck;
00337 A13 = so * sk - co * sp * ck;
00338
00339 A21 = -cp * sk;
00340 A22 = co * ck - so * sp * sk;
00341 A23 = so * ck + co * sp * sk;
00342
00343 A31 = sp;
00344 A32 = -so * cp;
00345 A33 = co * cp;
00346 }
00347
00349 void get_angles_opk_fix(T& omega, T& phi, T& kappa) const
00350 {
00351 omega = (T)atan2(-A32, A33);
00352 phi = (T)asin(A31);
00353 kappa = (T)atan2(-A21, A11);
00354 }
00356
00367
00368 void set_angles_opk_fix_ccw(T omega, T phi, T kappa)
00369 {
00370 T so = (T)sin((double)omega);
00371 T sp = (T)sin((double)phi);
00372 T sk = (T)sin((double)kappa);
00373
00374 T co = (T)cos((double)omega);
00375 T cp = (T)cos((double)phi);
00376 T ck = (T)cos((double)kappa);
00377
00378 A11 = ck * cp;
00379 A12 = -sk * co + ck * sp * so;
00380 A13 = sk * so + ck * sp * co;
00381
00382 A21 = sk * cp;
00383 A22 = ck * co + sk * sp * so;
00384 A23 = -ck * so + sk * sp * co;
00385
00386 A31 = -sp;
00387 A32 = cp * so;
00388 A33 = cp * co;
00389 }
00390
00392 void get_angles_opk_fix_ccw(T& omega, T& phi, T& kappa) const
00393 {
00394 omega = (T)atan2(A32, A33);
00395 phi = (T)asin(-A31);
00396 kappa = (T)atan2(A21, A11);
00397 }
00399
00410
00411 void set_angles_pok_fix(T phi, T omega, T kappa)
00412 {
00413 T sp = (T)sin((double)phi);
00414 T so = (T)sin((double)omega);
00415 T sk = (T)sin((double)kappa);
00416
00417 T cp = (T)cos((double)phi);
00418 T co = (T)cos((double)omega);
00419 T ck = (T)cos((double)kappa);
00420
00421 A11 = cp * ck - sp * so * sk;
00422 A12 = -co * sk;
00423 A13 = sp * ck + cp * so * sk;
00424
00425 A21 = cp * sk + sp * so * ck;
00426 A22 = co * ck;
00427 A23 = sp * sk - cp * so * ck;
00428
00429 A31 = -sp * co;
00430 A32 = so;
00431 A33 = cp * co;
00432 }
00433
00435 void get_angles_pok_fix(T& phi, T& omega, T& kappa) const
00436 {
00437 phi = (T)atan2(-A31, A33);
00438 omega = (T)asin(A32);
00439 kappa = (T)atan2(-A12, A22);
00440 }
00442
00454
00455 void set_angles_opk_rot(T omega, T phi, T kappa)
00456 {
00457 T so = (T)sin((double)omega);
00458 T sp = (T)sin((double)phi);
00459 T sk = (T)sin((double)kappa);
00460
00461 T co = (T)cos((double)omega);
00462 T cp = (T)cos((double)phi);
00463 T ck = (T)cos((double)kappa);
00464
00465 A11 = cp * ck;
00466 A12 = -cp * sk;
00467 A13 = sp;
00468
00469 A21 = co * sk + so * sp * ck;
00470 A22 = co * ck - so * sp * sk;
00471 A23 = -so * cp;
00472
00473 A31 = so * sk - co * sp * ck;
00474 A32 = so * ck + co * sp * sk;
00475 A33 = co * cp;
00476 }
00477
00479 void get_angles_opk_rot(T& omega, T& phi, T& kappa) const
00480 {
00481 omega = (T)atan2(-A23, A33);
00482 phi = (T)asin(A13);
00483 kappa = (T)atan2(-A12, A11);
00484 }
00486
00500
00501 void set_angles_pok_rot(T phi, T omega, T kappa)
00502 {
00503 T sp = (T)sin((double)phi);
00504 T so = (T)sin((double)omega);
00505 T sk = (T)sin((double)kappa);
00506
00507 T cp = (T)cos((double)phi);
00508 T co = (T)cos((double)omega);
00509 T ck = (T)cos((double)kappa);
00510
00511 A11 = cp * ck + sp * so * sk;
00512 A12 = -cp * sk + sp * so * ck;
00513 A13 = sp * co;
00514
00515 A21 = co * sk;
00516 A22 = co * ck;
00517 A23 = -so;
00518
00519 A31 = -sp * ck + cp * so * sk;
00520 A32 = sp * sk + cp * so * ck;
00521 A33 = cp * co;
00522 }
00523
00525 void get_angles_pok_rot(T& phi, T& omega, T& kappa) const
00526 {
00527 phi = (T)atan2(A13, A33);
00528 omega = (T)asin(-A23);
00529 kappa = (T)atan2(A21, A22);
00530 }
00532
00545
00546 void set_angles_australis(T az, T elev, T roll)
00547 {
00548 T sa = (T)sin((double)az);
00549 T se = (T)sin((double)elev);
00550 T sr = (T)sin((double)roll);
00551
00552 T ca = (T)cos((double)az);
00553 T ce = (T)cos((double)elev);
00554 T cr = (T)cos((double)roll);
00555
00556 A11 = ca * cr + sa * se * sr;
00557 A12 = -ca * sr + sa * se * cr;
00558 A13 = sa * ce;
00559
00560 A21 = sa * cr - ca * se * sr;
00561 A22 = -sa * sr - ca * se * cr;
00562 A23 = -ca * ce;
00563
00564 A31 = ce * sr;
00565 A32 = ce * cr;
00566 A33 = -se;
00567 }
00568
00570 void get_angles_australis(T& az, T& elev, T& roll) const
00571 {
00572 az = (T)atan2(A13, -A23);
00573 elev = -(T)asin(A33);
00574 roll = (T)atan2(A31, A32);
00575 }
00576
00586
00587 void set_angles_small(T eps_x, T eps_y, T eps_z)
00588 {
00589 A11 = (T)1.;
00590 A12 = -eps_z;
00591 A13 = eps_y;
00592
00593 A21 = eps_z;
00594 A22 = (T)1.;
00595 A23 = -eps_x;
00596
00597 A31 = -eps_y;
00598 A32 = eps_x;
00599 A33 = (T)1.;
00600 }
00601
00603 void get_angles_small(T& eps_x, T& eps_y, T& eps_z) const
00604 {
00605 eps_x = A32;
00606 eps_y = A13;
00607 eps_z = A21;
00608 }
00610
00619
00620 void set_upper_elem(T a12, T a13, T a23)
00621 {
00622 A12 = a12;
00623 A13 = a13;
00624 A23 = a23;
00625
00626 A11 = (T)sqrt(1. - a12 * a12 - a13 * a13);
00627 A33 = (T)sqrt(1. - a13 * a13 - a23 * a23);
00628
00629 T d = 1. - a13 * a13;
00630
00631 A21 = -(a12 * A33 + A11 * a13 * a23) / d;
00632 A22 = (A11 * A33 - a12 * a13 * a23) / d;
00633 A31 = a12 * a23 - a13 * A22;
00634 A32 = a13 * A21 - A11 * a23;
00635 }
00636
00638 void get_upper_elem(T& a12, T& a13, T& a23) const
00639 {
00640 a12 = A12;
00641 a13 = A13;
00642 a23 = A23;
00643 }
00645
00660
00661 void set_rodrigues_matrix(T a, T b, T c)
00662 {
00663 T aa = a * a;
00664 T bb = b * b;
00665 T cc = c * c;
00666
00667 T d = (aa + bb + cc) / 4. + 1.;
00668
00669 A11 = (1. + ( aa - bb - cc) / 4.) / d;
00670 A12 = ((a * b) / 2. - c) / d;
00671 A13 = ((a * c) / 2. + b) / d;
00672
00673 A21 = ((a * b) / 2. + c) / d;
00674 A22 = (1. + (-aa + bb - cc) / 4.) / d;
00675 A23 = ((b * c) / 2. - a) / d;
00676
00677 A31 = ((a * c) / 2. - b) / d;
00678 A32 = ((b * c) / 2. + a) / d;
00679 A33 = (1. + (-aa - bb + cc) / 4.) / d;
00680 }
00681
00683 void get_rodrigues_matrix(T& a, T& b, T& c) const
00684 {
00685 T d2 = 2. / (A11 + A22 + A33 + 1);
00686
00687 a = (A32 - A23) * d2;
00688 b = (A13 - A31) * d2;
00689 c = (A21 - A12) * d2;
00690 }
00692
00713
00714 void set_quaternion(T q0, T q1, T q2, T q3)
00715 {
00716 T q0q0, q0q1, q0q2, q0q3,
00717 q1q1, q1q2, q1q3,
00718 q2q2, q2q3,
00719 q3q3;
00720
00721 q0q0 = q0 * q0;
00722 q0q1 = q0 * q1;
00723 q0q2 = q0 * q2;
00724 q0q3 = q0 * q3;
00725 q1q1 = q1 * q1;
00726 q1q2 = q1 * q2;
00727 q1q3 = q1 * q3;
00728 q2q2 = q2 * q2;
00729 q2q3 = q2 * q3;
00730 q3q3 = q3 * q3;
00731
00732 A11 = q0q0 + q1q1 - q2q2 - q3q3;
00733 A12 = 2.0 * (q1q2 - q0q3);
00734 A13 = 2.0 * (q1q3 + q0q2);
00735 A21 = 2.0 * (q1q2 + q0q3);
00736 A22 = q0q0 - q1q1 + q2q2 - q3q3;
00737 A23 = 2.0 * (q2q3 - q0q1);
00738 A31 = 2.0 * (q1q3 - q0q2);
00739 A32 = 2.0 * (q2q3 + q0q1);
00740 A33 = q0q0 - q1q1 - q2q2 + q3q3;
00741 }
00742
00744 void set_quaternion(const vector<T>& q)
00745 {
00746 assert(q.size() == 4);
00747 set_quaternion(q[0], q[1], q[2], q[3]);
00748 }
00749
00751 void get_quaternion(T& q0, T& q1, T& q2, T& q3) const
00752 {
00753 q0 = 0.5 * (T)sqrt(_a[0][0] + _a[1][1] + _a[2][2] + 1.0);
00754
00755 if(q0 != 0.0)
00756 {
00757 q1 = -(_a[1][2] - _a[2][1]) / q0 / 4.0;
00758 q2 = (_a[0][2] - _a[2][0]) / q0 / 4.0;
00759 q3 = -(_a[0][1] - _a[1][0]) / q0 / 4.0;
00760 }
00761 else
00762 {
00763 T q1q2, q1q3, q2q3;
00764
00765 q1q2 = (_a[0][1] + _a[1][0]) / 4.0;
00766 q1q3 = (_a[0][2] + _a[2][0]) / 4.0;
00767 q2q3 = (_a[1][2] + _a[2][1]) / 4.0;
00768
00769 if (q1q2 != 0.0)
00770 {
00771 q3 = (T)sqrt(q1q3 * q2q3 / q1q2);
00772 q1 = q1q3 / q3;
00773 q2 = q2q3 / q3;
00774 }
00775 else if (q1q3 != 0.0)
00776 {
00777 q2 = (T)sqrt(q1q2 * q2q3 / q1q3);
00778 q1 = q1q2 / q2;
00779 q3 = q2q3 / q2;
00780 }
00781 else if (q2q3 != 0.0)
00782 {
00783 q1 = (T)sqrt(q1q2 *q1q3 / q2q3);
00784 q2 = q1q2 / q1;
00785 q3 = q1q3 / q1;
00786 }
00787 else
00788 {
00789
00790
00791
00792
00793
00794
00795
00796
00797
00798
00799
00800
00801 if (_a[0][0] > 0.0)
00802 {
00803 q1 = 1.0; q2 = q3 = 0.0;
00804 }
00805 else if (_a[1][1] > 0.0)
00806 {
00807 q2 = 1.0; q1 = q3 = 0.0;
00808 }
00809 else
00810 {
00811 q3 = 1.0; q1 = q2 = 0.0;
00812 }
00813 }
00814 }
00815 }
00816
00818 void get_quaternion(vector<T>& q) const
00819 {
00820 assert(q.size() == 4);
00821 get_quaternion(q[0], q[1], q[2], q[3]);
00822 }
00824
00838
00839 void set_about_line(T a, T b, T c, T t)
00840 {
00841 T st = (T)sin((double)t);
00842 T ct = (T)cos((double)t);
00843
00844 T e = 1. - ct;
00845
00846 A11 = a * a * e + ct;
00847 A12 = a * b * e - c * st;
00848 A13 = a * b * e + b * st;
00849
00850 A21 = a * b * e + c * st;
00851 A22 = b * b * e + ct;
00852 A23 = b * c * e - a * st;
00853
00854 A31 = a * c * e - b * st;
00855 A32 = b * c * e + a * st;
00856 A33 = c * c * e + ct;
00857 }
00858
00860 void get_about_line(T& a, T& b, T& c, T& t) const
00861 {
00862 t = acos((A11 + A22 + A33 - 1.) / 2.);
00863
00864 T f = 1. / (2. * sin(t));
00865
00866 a = f * (A32 - A23);
00867 b = f * (A13 - A31);
00868 c = f * (A21 - A12);
00869 }
00871
00877
00878 void set(const T* v, ERotMatrix3::ecode type)
00879 {
00880 assert(v != 0);
00881
00882 switch (type)
00883 {
00884 case ERotMatrix3::UNDEFINED:
00885 cerr << "RotMatrix3<T>::set: type undefined" << endl;
00886 assert(false);
00887 break;
00888
00889 case ERotMatrix3::ROTATION_MATRIX:
00890 case ERotMatrix3::DIRECTION_COSINES:
00891 set_matrix(v);
00892 break;
00893
00894 case ERotMatrix3::ANGLES_OPK_FIX:
00895 set_angles_opk_fix(v[0], v[1], v[2]);
00896 break;
00897
00898 case ERotMatrix3::ANGLES_OPK_FIX_CCW:
00899 set_angles_opk_fix_ccw(v[0], v[1], v[2]);
00900 break;
00901
00902 case ERotMatrix3::ANGLES_POK_FIX:
00903 set_angles_pok_fix(v[0], v[1], v[2]);
00904 break;
00905
00906 case ERotMatrix3::ANGLES_OPK_ROT:
00907 set_angles_opk_rot(v[0], v[1], v[2]);
00908 break;
00909
00910 case ERotMatrix3::ANGLES_POK_ROT:
00911 set_angles_pok_rot(v[0], v[1], v[2]);
00912 break;
00913
00914 case ERotMatrix3::ANGLES_AUSTRALIS:
00915 set_angles_australis(v[0], v[1], v[2]);
00916 break;
00917
00918 case ERotMatrix3::ANGLES_SMALL:
00919 set_angles_small(v[0], v[1], v[2]);
00920 break;
00921
00922 case ERotMatrix3::UPPER_DIAG_ELEM:
00923 set_upper_elem(v[0], v[1], v[2]);
00924 break;
00925
00926 case ERotMatrix3::RODRIGUES_MATRIX:
00927 set_rodrigues_matrix(v[0], v[1], v[2]);
00928 break;
00929
00930 case ERotMatrix3::UNIT_QUATERNION:
00931 set_quaternion(v[0], v[1], v[2], v[3]);
00932 break;
00933
00934 case ERotMatrix3::ROTATION_ABOUT_LINE:
00935 set_about_line(v[0], v[1], v[2], v[3]);
00936 break;
00937
00938 default:
00939 cerr << "RotMatrix3<T>::set: bad type: " << type << endl;
00940 assert(false);
00941 }
00942 }
00943
00945 void get(T* v, ERotMatrix3::ecode type) const
00946 {
00947 assert(v != 0);
00948
00949 switch (type)
00950 {
00951 case ERotMatrix3::UNDEFINED:
00952 cerr << "RotMatrix3<T>::get: type undefined" << endl;
00953 assert(false);
00954 break;
00955
00956 case ERotMatrix3::ROTATION_MATRIX:
00957 case ERotMatrix3::DIRECTION_COSINES:
00958 get_matrix(v);
00959 break;
00960
00961 case ERotMatrix3::ANGLES_OPK_FIX:
00962 get_angles_opk_fix(v[0], v[1], v[2]);
00963 break;
00964
00965 case ERotMatrix3::ANGLES_OPK_FIX_CCW:
00966 get_angles_opk_fix_ccw(v[0], v[1], v[2]);
00967 break;
00968
00969 case ERotMatrix3::ANGLES_OPK_ROT:
00970 get_angles_opk_rot(v[0], v[1], v[2]);
00971 break;
00972
00973 case ERotMatrix3::ANGLES_POK_ROT:
00974 get_angles_pok_rot(v[0], v[1], v[2]);
00975 break;
00976
00977 case ERotMatrix3::ANGLES_AUSTRALIS:
00978 get_angles_australis(v[0], v[1], v[2]);
00979 break;
00980
00981 case ERotMatrix3::ANGLES_SMALL:
00982 get_angles_small(v[0], v[1], v[2]);
00983 break;
00984
00985 case ERotMatrix3::UPPER_DIAG_ELEM:
00986 get_upper_elem(v[0], v[1], v[2]);
00987 break;
00988
00989 case ERotMatrix3::RODRIGUES_MATRIX:
00990 get_rodrigues_matrix(v[0], v[1], v[2]);
00991 break;
00992
00993 case ERotMatrix3::UNIT_QUATERNION:
00994 get_quaternion(v[0], v[1], v[2], v[3]);
00995 break;
00996
00997 case ERotMatrix3::ROTATION_ABOUT_LINE:
00998 get_about_line(v[0], v[1], v[2], v[3]);
00999 break;
01000
01001 default:
01002 cerr << "RotMatrix3<T>::get: bad type: " << type << endl;
01003 assert(false);
01004 }
01005 }
01007
01013
01014 void set(const vector<T>& v, ERotMatrix3::ecode type)
01015 {
01016 assert(!v.empty());
01017
01018 ERotMatrix3 map;
01019 assert(v.size() == map.size(type));
01020 set(&v[0], type);
01021 }
01022
01024 void get(vector<T>& v, ERotMatrix3::ecode type) const
01025 {
01026 ERotMatrix3 map;
01027 v.resize(map.size(type));
01028 get(&v[0], type);
01029 }
01031
01033 void transpose();
01034
01038 void inverse() { transpose(); }
01039
01046 T det() const;
01047
01052 bool is_equal(RotMatrix3& A, T eps=0) const;
01053
01054 private:
01056 T _a[3][3];
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068 };
01069
01070
01071
01072
01076 template<class T>
01077 istream& operator>>(istream& is, RotMatrix3<T>& M)
01078 {
01079 is >> M.A11 >> M.A12 >> M.A13;
01080 is >> M.A21 >> M.A22 >> M.A23;
01081 is >> M.A31 >> M.A32 >> M.A33;
01082
01083 return is;
01084 }
01085
01086
01090 template<class T>
01091 ostream& operator<<(ostream& os, const RotMatrix3<T>& M)
01092 {
01093 os << M.A11 << " " << M.A12 << " " << M.A13 << endl;
01094 os << M.A21 << " " << M.A22 << " " << M.A23 << endl;
01095 os << M.A31 << " " << M.A32 << " " << M.A33 << endl;
01096
01097 return os;
01098 }
01099
01100
01104 template<class T>
01105 RotMatrix3<T> operator*(const RotMatrix3<T>& A, const RotMatrix3<T>& B)
01106 {
01107 RotMatrix3<T> C;
01108
01109 C.A11 = A.A11 * B.A11 + A.A12 * B.A21 + A.A13 * B.A31;
01110 C.A12 = A.A11 * B.A12 + A.A12 * B.A22 + A.A13 * B.A32;
01111 C.A13 = A.A11 * B.A13 + A.A12 * B.A23 + A.A13 * B.A33;
01112
01113 C.A21 = A.A21 * B.A11 + A.A22 * B.A21 + A.A23 * B.A31;
01114 C.A22 = A.A21 * B.A12 + A.A22 * B.A22 + A.A23 * B.A32;
01115 C.A23 = A.A21 * B.A13 + A.A22 * B.A23 + A.A23 * B.A33;
01116
01117 C.A31 = A.A31 * B.A11 + A.A32 * B.A21 + A.A33 * B.A31;
01118 C.A32 = A.A31 * B.A12 + A.A32 * B.A22 + A.A33 * B.A32;
01119 C.A33 = A.A31 * B.A13 + A.A32 * B.A23 + A.A33 * B.A33;
01120
01121
01122
01123
01124
01125
01126
01127
01128
01129
01130
01131
01132
01133
01134 return C;
01135 }
01136
01137
01138
01139
01140
01141
01142 template<class T>
01143 void RotMatrix3<T>::identity()
01144 {
01145 A11 = A22 = A33 = T(1);
01146 A12 = A13 = A21 = A23 = A31 = A32 = T(0);
01147 }
01148
01149
01150
01151
01152 template<class T>
01153 RotMatrix3<T>::RotMatrix3(const RotMatrix3<T>& M)
01154 {
01155 memcpy(_a, &M._a, sizeof(_a));
01156 }
01157
01158
01159
01160
01161 template<class T>
01162 RotMatrix3<T>& RotMatrix3<T>::operator=(const RotMatrix3<T>& M)
01163 {
01164 memcpy(_a, &M._a, sizeof(_a));
01165 return *this;
01166 }
01167
01168
01169
01170
01171 template<class T>
01172 void RotMatrix3<T>::set(char axis, T a)
01173 {
01174 identity();
01175
01176 T sin_a = (T)sin((double)a);
01177 T cos_a = (T)cos((double)a);
01178
01179 int m = 0;
01180 switch (axis)
01181 {
01182 case 'x': m = 1; break;
01183 case 'y': m = 2; break;
01184 case 'z': m = 0; break;
01185 default:
01186 cerr << "RotMatrix3<T>::set: bad axis " << axis << endl;
01187 return;
01188 }
01189 int n = (m + 1) % 3;
01190
01191 _a[m][m] = cos_a;
01192 _a[n][n] = cos_a;
01193 _a[m][n] = -sin_a;
01194 _a[n][m] = sin_a;
01195
01196
01197
01198 return;
01199 }
01200
01201
01202
01203
01204 template<class T>
01205 void RotMatrix3<T>::transpose()
01206 {
01207 T t;
01208
01209 t = A21; A21 = A12; A12 = t;
01210 t = A31; A31 = A13; A13 = t;
01211 t = A32; A32 = A23; A23 = t;
01212 }
01213
01214
01215
01216
01217 template<class T>
01218 T RotMatrix3<T>::det() const
01219 {
01220 return A11 * A22 * A33 + A12 * A23 * A31 + A13 * A21 * A32 -
01221 A13 * A22 * A31 - A11 * A23 * A32 - A12 * A21 * A33;
01222 }
01223
01224
01225
01226
01227 template<class T>
01228 bool RotMatrix3<T>::is_equal(RotMatrix3<T>& A, T eps) const
01229 {
01230 for (int i = 0; i < 3; i++)
01231 for (int j = 0; j < 3; j++)
01232 if (fabs(_a[i][j] - A._a[i][j]) > eps) return false;
01233
01234 return true;
01235 }
01236
01237
01238
01239
01240 }
01241
01242 #endif