00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef _SB_MATRIX_
00024 #define _SB_MATRIX_
00025
00026 #include <Inventor/SbBase.h>
00027 #include <Inventor/STL/iostream>
00028 #include <Inventor/SbVec.h>
00029 #include <string.h>
00030
00031 class SbLine;
00032 class SbLined;
00033 class SbRotation;
00034 class SbRotationd;
00035 class SbMatrixd;
00036
00037 typedef float SbMat[4][4];
00038 typedef float SbMat3[3][3];
00039 typedef double SbMatd[4][4];
00040
00042
00043
00044
00045
00046
00048
00049
00050
00309 class SbMatrix {
00310 public:
00311
00315 SbMatrix() { memset(matrix, 0, sizeof(SbMat)); }
00316
00320 SbMatrix(float a11, float a12, float a13, float a14,
00321 float a21, float a22, float a23, float a24,
00322 float a31, float a32, float a33, float a34,
00323 float a41, float a42, float a43, float a44);
00324
00328 SbMatrix(const SbMat &m);
00329
00333 void setValue(const SbMat &m);
00334
00339 void setValue(const float *pMat)
00340 {
00341 matrix[0][0] = *pMat++ ;
00342 matrix[0][1] = *pMat++ ;
00343 matrix[0][2] = *pMat++ ;
00344 matrix[0][3] = *pMat++ ;
00345
00346 matrix[1][0] = *pMat++ ;
00347 matrix[1][1] = *pMat++ ;
00348 matrix[1][2] = *pMat++ ;
00349 matrix[1][3] = *pMat++ ;
00350
00351 matrix[2][0] = *pMat++ ;
00352 matrix[2][1] = *pMat++ ;
00353 matrix[2][2] = *pMat++ ;
00354 matrix[2][3] = *pMat++ ;
00355
00356 matrix[3][0] = *pMat++ ;
00357 matrix[3][1] = *pMat++ ;
00358 matrix[3][2] = *pMat++ ;
00359 matrix[3][3] = *pMat++ ;
00360 } ;
00361
00365 void setValue(const SbMatrixd &md) ;
00366
00367 public:
00368
00372 void makeIdentity();
00373
00377 static SbMatrix identity();
00378
00382 void setRotate(const SbRotation &q);
00383
00387 void setScale(float s);
00388
00392 void setScale(const SbVec3f &s);
00393
00397 void setTranslate(const SbVec3f &t);
00398
00404 void setTransform(const SbVec3f &translation,
00405 const SbRotation &rotation,
00406 const SbVec3f &scaleFactor,
00407 const SbRotation &scaleOrientation,
00408 const SbVec3f ¢er);
00409
00414 void setTransform(const SbVec3f &t, const SbRotation &r, const SbVec3f &s);
00415
00421 void setTransform(const SbVec3f &t, const SbRotation &r,
00422 const SbVec3f &s, const SbRotation &so);
00423
00433 void getTransform(SbVec3f &translation,
00434 SbRotation &rotation,
00435 SbVec3f &scaleFactor,
00436 SbRotation &scaleOrientation,
00437 const SbVec3f ¢er) const;
00438
00444 void getTransform(SbVec3f &t, SbRotation &r,
00445 SbVec3f &s, SbRotation &so) const;
00446
00447
00448
00452 void getValue(SbMat &m) const;
00453
00454 #ifdef _WIN32
00455
00458 SbMat &getValue() { SbMat &rMat = matrix; return rMat; }
00459 #endif
00460
00464 const SbMat &getValue() const { return matrix; }
00465
00470 float det3(int r1, int r2, int r3, int c1, int c2, int c3) const;
00471
00475 float det3() const { return det3(0, 1, 2, 0, 1, 2); }
00476
00480 float det4() const;
00481
00488 SbBool factor(SbMatrix &r, SbVec3f &s, SbMatrix &u,
00489 SbVec3f &t, SbMatrix &proj) const;
00490
00496 SbMatrix inverse() const;
00497
00499 void translate(const SbVec3f& translation);
00500
00502 void scale(const SbVec3f& scaleFactor);
00503
00508 SbBool LUDecomposition(int index[4], float &d);
00509
00514 void LUBackSubstitution(int index[4], float b[4]) const;
00515
00520 SbMatrix transpose() const;
00521
00522
00523
00524
00529 SbMatrix &multRight(const SbMatrix &m);
00530
00535 SbMatrix &multLeft(const SbMatrix &m);
00536
00544 void multMatrixVec(const SbVec3f &src, SbVec3f &dst) const;
00545
00556 inline void multVecMatrix(const SbVec3f &src, SbVec3f &dst) const
00557 {
00558 float x,y,z,w;
00559
00560 x = src[0]*matrix[0][0] + src[1]*matrix[1][0] +
00561 src[2]*matrix[2][0] + matrix[3][0];
00562 y = src[0]*matrix[0][1] + src[1]*matrix[1][1] +
00563 src[2]*matrix[2][1] + matrix[3][1];
00564 z = src[0]*matrix[0][2] + src[1]*matrix[1][2] +
00565 src[2]*matrix[2][2] + matrix[3][2];
00566 w = src[0]*matrix[0][3] + src[1]*matrix[1][3] +
00567 src[2]*matrix[2][3] + matrix[3][3];
00568
00569 w = 1.f/w;
00570 dst.setValue(x*w, y*w, z*w);
00571 }
00572
00578 void multMatrixVec(const SbVec3f &src, SbVec4f &dst) const;
00579
00586 void multVecMatrix(const SbVec3f &src, SbVec4f &dst) const;
00587
00600 void multDirMatrix(const SbVec3f &src, SbVec3f &dst) const;
00601
00607 void multLineMatrix(const SbLine &src, SbLine &dst) const;
00608
00609
00610
00611
00615 void print(FILE *fp) const;
00616
00617
00621 operator float *() { return &matrix[0][0]; }
00622
00626 operator SbMat &() { return matrix; }
00627
00631 float * operator [](int i) { return &matrix[i][0]; }
00632
00636 const float * operator [](int i) const { return &matrix[i][0]; }
00637
00641 SbMatrix & operator =(const SbMat &m);
00642
00646 SbMatrix & operator =(const SbMatrix &m);
00647
00651 SbMatrix & operator =(const SbRotation &q) { setRotate(q); return *this; }
00652
00657 SbMatrix & operator *=(const SbMatrix &m) { return multRight(m); }
00658
00663 SbVec4f operator *(const SbVec4f& v);
00664
00669 friend SbVec4f operator *(const SbVec4f &v, const SbMatrix &m);
00670
00674 friend SbMatrix operator *(const SbMatrix &m1, const SbMatrix &m2);
00675
00679 friend int operator ==(const SbMatrix &m1, const SbMatrix &m2);
00683 friend int operator !=(const SbMatrix &m1, const SbMatrix &m2)
00684 { return !(m1 == m2); }
00685
00689 SbBool equals(const SbMatrix &m, float tolerance) const;
00690
00694 bool isInvertible() const;
00695
00696 private:
00700 void jacobi3(float evalues[3], SbVec3f evectors[3], int &rots) const;
00701
00702 private:
00703
00704 SbMat matrix;
00705
00706 SbBool affine_inverse(const SbMatrix &in, SbMatrix &out) const;
00707
00708
00709 };
00710
00712
00713
00714
00715
00716
00718
00756 class SbMatrixd {
00757 public:
00758
00762 SbMatrixd() { memset(matrix, 0, sizeof(SbMatd)); }
00763
00767 SbMatrixd(double a11, double a12, double a13, double a14,
00768 double a21, double a22, double a23, double a24,
00769 double a31, double a32, double a33, double a34,
00770 double a41, double a42, double a43, double a44);
00771
00775 SbMatrixd(const SbMatd &m);
00776
00780 void setValue(const SbMatd &m);
00781
00786 void setValue(const double *pMat)
00787 {
00788 matrix[0][0] = *pMat++ ;
00789 matrix[0][1] = *pMat++ ;
00790 matrix[0][2] = *pMat++ ;
00791 matrix[0][3] = *pMat++ ;
00792
00793 matrix[1][0] = *pMat++ ;
00794 matrix[1][1] = *pMat++ ;
00795 matrix[1][2] = *pMat++ ;
00796 matrix[1][3] = *pMat++ ;
00797
00798 matrix[2][0] = *pMat++ ;
00799 matrix[2][1] = *pMat++ ;
00800 matrix[2][2] = *pMat++ ;
00801 matrix[2][3] = *pMat++ ;
00802
00803 matrix[3][0] = *pMat++ ;
00804 matrix[3][1] = *pMat++ ;
00805 matrix[3][2] = *pMat++ ;
00806 matrix[3][3] = *pMat++ ;
00807 } ;
00808
00812 void setValue(const SbMatrix &m) ;
00813
00814 public:
00815
00819 void makeIdentity();
00820
00824 static SbMatrixd identity();
00825
00829 void setRotate(const SbRotationd &q);
00830
00834 void setScale(double s);
00835
00839 void setScale(const SbVec3d &s);
00840
00844 void setTranslate(const SbVec3d &t);
00845
00851 void setTransform(const SbVec3d &translation,
00852 const SbRotationd &rotation,
00853 const SbVec3d &scaleFactor,
00854 const SbRotationd &scaleOrientation,
00855 const SbVec3d ¢er);
00856
00861 void setTransform(const SbVec3d &t, const SbRotationd &r, const SbVec3d &s);
00862
00868 void setTransform(const SbVec3d &t, const SbRotationd &r,
00869 const SbVec3d &s, const SbRotationd &so);
00870
00880 void getTransform(SbVec3d &translation,
00881 SbRotationd &rotation,
00882 SbVec3d &scaleFactor,
00883 SbRotationd &scaleOrientation,
00884 const SbVec3d ¢er) const;
00885
00891 void getTransform(SbVec3d &t, SbRotationd &r,
00892 SbVec3d &s, SbRotationd &so) const;
00893
00894
00895
00896
00900 void getValue(SbMatd &m) const;
00901
00902 #ifdef _WIN32
00903
00907 SbMatd &getValue() { SbMatd &rMat = matrix; return rMat; }
00908 #endif
00909
00913 const SbMatd &getValue() const { return matrix; }
00914
00919 double det3(int r1, int r2, int r3, int c1, int c2, int c3) const;
00920
00924 double det3() const { return det3(0, 1, 2, 0, 1, 2); }
00925
00929 double det4() const;
00930
00937 SbBool factor(SbMatrixd &r, SbVec3d &s, SbMatrixd &u,
00938 SbVec3d &t, SbMatrixd &proj) const;
00939
00945 SbMatrixd inverse() const;
00946
00948 void translate(const SbVec3d& translation);
00949
00951 void scale(const SbVec3d& scaleFactor);
00952
00957 SbBool LUDecomposition(int index[4], double &d);
00958
00963 void LUBackSubstitution(int index[4], double b[4]) const;
00964
00969 SbMatrixd transpose() const;
00970
00971
00972
00973
00978 SbMatrixd &multRight(const SbMatrixd &m);
00979
00984 SbMatrixd &multLeft(const SbMatrixd &m);
00985
00993 void multMatrixVec(const SbVec3d &src, SbVec3d &dst) const;
00994
01005 void multVecMatrix(const SbVec3d &src, SbVec3d &dst) const;
01006
01019 void multDirMatrix(const SbVec3d &src, SbVec3d &dst) const;
01020
01026 void multLineMatrix(const SbLined &src, SbLined &dst) const;
01027
01032 SbVec4d operator *(const SbVec4d& v);
01033
01034
01035
01036
01040 void print(FILE *fp) const;
01041
01042
01046 operator double *() { return &matrix[0][0]; }
01047
01051 operator SbMatd &() { return matrix; }
01052
01056 double * operator [](int i) { return &matrix[i][0]; }
01057
01061 const double * operator [](int i) const { return &matrix[i][0]; }
01062
01066 SbMatrixd & operator =(const SbMatd &m);
01067
01071 SbMatrixd & operator =(const SbMatrixd &m);
01072
01076 SbMatrixd & operator =(const SbRotationd &q) { setRotate(q); return *this; }
01077
01082 SbMatrixd & operator *=(const SbMatrixd &m) { return multRight(m); }
01083
01088 friend SbVec4d operator *(const SbVec4d &v, const SbMatrixd &m);
01089
01093 friend SbMatrixd operator *(const SbMatrixd &m1, const SbMatrixd &m2);
01094
01098 friend int operator ==(const SbMatrixd &m1, const SbMatrixd &m2);
01102 friend int operator !=(const SbMatrixd &m1, const SbMatrixd &m2)
01103 { return !(m1 == m2); }
01104
01108 SbBool equals(const SbMatrixd &m, double tolerance) const;
01109
01113 bool isInvertible() const;
01114
01115 private:
01116
01120 void jacobi3(double evalues[3], SbVec3d evectors[3], int &rots) const;
01121
01122 private:
01123
01124 SbMatd matrix;
01125
01126 SbBool affine_inverse(const SbMatrixd &in, SbMatrixd &out) const;
01127
01128
01129 };
01130
01132
01133
01134
01135
01136
01138
01139
01140
01175 class SbMatrix3 {
01176 public:
01177
01181 SbMatrix3() { memset(matrix, 0, sizeof(SbMat3)); }
01182
01186 SbMatrix3(float a11, float a12, float a13,
01187 float a21, float a22, float a23,
01188 float a31, float a32, float a33);
01189
01193 SbMatrix3(const SbMat3 &m);
01194
01198 void setValue(const SbMat3 &m);
01199
01204 void setValue(const float *pMat)
01205 {
01206 matrix[0][0] = *pMat++ ;
01207 matrix[0][1] = *pMat++ ;
01208 matrix[0][2] = *pMat++ ;
01209
01210 matrix[1][0] = *pMat++ ;
01211 matrix[1][1] = *pMat++ ;
01212 matrix[1][2] = *pMat++ ;
01213
01214 matrix[2][0] = *pMat++ ;
01215 matrix[2][1] = *pMat++ ;
01216 matrix[2][2] = *pMat++ ;
01217 } ;
01218
01222 void makeIdentity();
01223
01227 static SbMatrix3 identity();
01228
01232 void setRotate(const SbRotation &q);
01233
01237 void setScale(float s);
01238
01242 void setScale(const SbVec3f &s);
01243
01247 void getValue(SbMat3 &m) const;
01248
01252 const SbMat3 &getValue() const { return matrix; }
01253
01258 SbMatrix3 &multRight(const SbMatrix3 &m);
01259
01264 SbMatrix3 &multLeft(const SbMatrix3 &m);
01265
01270 void multMatrixVec(const SbVec3f &src, SbVec3f &dst) const;
01271
01276 void multVecMatrix(const SbVec3f &src, SbVec3f &dst) const;
01277
01281 void print(FILE *fp) const;
01282
01286 operator float *() { return &matrix[0][0]; }
01287
01291 operator SbMat3 &() { return matrix; }
01292
01296 float * operator [](int i) { return &matrix[i][0]; }
01297
01301 const float * operator [](int i) const { return &matrix[i][0]; }
01302
01307 SbVec3f operator *(const SbVec3f &v);
01308
01312 SbMatrix3 & operator =(const SbMat3 &m);
01313
01317 SbMatrix3 & operator =(const SbMatrix3 &m);
01318
01322 SbMatrix3 & operator =(const SbRotation &q) { setRotate(q); return *this; }
01323
01328 SbMatrix3 & operator *=(const SbMatrix3 &m) { return multRight(m); }
01329
01334 friend SbVec3f operator *(const SbVec3f &v, const SbMatrix3 &m);
01335
01339 friend SbMatrix3 operator *(const SbMatrix3 &m1, const SbMatrix3 &m2);
01340
01344 friend int operator ==(const SbMatrix3 &m1, const SbMatrix3 &m2);
01348 friend int operator !=(const SbMatrix3 &m1, const SbMatrix3 &m2)
01349 { return !(m1 == m2); }
01350
01351 private:
01352 SbMat3 matrix;
01353 };
01354
01355 inline std::ostream& operator << (std::ostream& os, const SbMatrix& mat)
01356 {
01357 for ( int j = 0; j < 4; j++ )
01358 {
01359 os << "[ ";
01360 for ( int i = 0; i < 4; i++ )
01361 os << mat[j][i] << " ";
01362 os << "]" << std::endl;
01363 }
01364 return os;
01365 }
01366
01367 inline std::ostream& operator << (std::ostream& os, const SbMatrix3& mat)
01368 {
01369 for ( int j = 0; j < 3; j++ )
01370 {
01371 os << "[ ";
01372 for ( int i = 0; i < 3; i++ )
01373 os << mat[j][i] << " ";
01374 os << "]" << std::endl;
01375 }
01376 return os;
01377 }
01378
01379 #endif
01380
01381
01382