00001 #ifndef dvm3_matrix_h_INCLUDED
00002 #define dvm3_matrix_h_INCLUDED
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <dvm3/dvm3_vector.h>
00037 #include <vm_math/vm_m3math.h>
00038 #include <iosfwd>
00039 #include <cstdio>
00040
00041
00042
00043
00044
00068 struct dvm3_MPOD
00069 {
00070 double m_[9];
00071 };
00074
00075
00076
00077
00104 class dvm3_Matrix
00105 {
00106 protected:
00107
00108 dvm3_MPOD data_;
00109 enum ENelts_ { ENelts = 9 };
00110 enum ERowOffset_ { ERow0 = 0, ERow1 = 3, ERow2 = 6 };
00111 enum EColStride_ { EColStride = 3 };
00112
00113 public:
00114
00115
00123 ~dvm3_Matrix();
00124
00128 dvm3_Matrix();
00129
00135 dvm3_Matrix( dvm3_Matrix const& m );
00136
00144 dvm3_Matrix( dvm3_Vector const& x,
00145 dvm3_Vector const& y,
00146 dvm3_Vector const& z );
00147
00155 dvm3_Matrix( double const x[],
00156 double const y[],
00157 double const z[] );
00160
00165
00173 void
00174 init_by_row( dvm3_Vector const& v1,
00175 dvm3_Vector const& v2,
00176 dvm3_Vector const& v3 );
00177
00185 void
00186 init_by_row( double const v1[],
00187 double const v2[],
00188 double const v3[] );
00189
00197 void
00198 init_by_col( dvm3_Vector const& v1,
00199 dvm3_Vector const& v2,
00200 dvm3_Vector const& v3 );
00201
00209 void
00210 init_by_col( double const v1[],
00211 double const v2[],
00212 double const v3[] );
00215
00220
00228 double const* operator[](int i) const;
00229
00230
00231
00239 double* operator[](int i);
00240
00241
00242
00251 double operator()(int i, int j) const;
00252
00261 double& operator()(int i, int j);
00264
00276 dvm3_Matrix& operator=( dvm3_Matrix const& rhs );
00277
00285 dvm3_Matrix& operator=( double rhs );
00286
00294 dvm3_Matrix& operator+=( dvm3_Matrix const& rhs );
00295
00303 dvm3_Matrix& operator-=( dvm3_Matrix const& rhs );
00304
00313 dvm3_Matrix& operator*=( dvm3_Matrix const& rhs );
00314
00323 dvm3_Matrix& operator/=( dvm3_Matrix const& rhs );
00324
00332 dvm3_Matrix& operator+=( double rhs );
00333
00341 dvm3_Matrix& operator-=( double rhs );
00342
00351 dvm3_Matrix& operator*=( double rhs );
00352
00361 dvm3_Matrix& operator/=( double rhs );
00364
00372 dvm3_Matrix& operator+();
00373
00377 dvm3_Matrix& operator-();
00378
00379
00390 void inject_row( int whichrow, dvm3_Vector const& new_row );
00391
00398 void inject_row( int whichrow, double const new_row[] );
00399
00406 void inject_col( int whichcol, dvm3_Vector const& col );
00407
00414 void inject_col( int whichcol, double const col[] );
00415
00422 void extract_row( int whichrow, dvm3_Vector& row ) const;
00423
00430 void extract_row( int whichrow, double row[] ) const;
00431
00438 void extract_col( int whichcol, dvm3_Vector& col ) const;
00439
00446 void extract_col( int whichcol, double* col ) const;
00447
00450
00464 void dyad_product( dvm3_Vector const& v1, dvm3_Vector const& v2 );
00465
00475 void dyad_product( double const v1[], double const v2[] );
00478
00494 void lincomb( double c1, dvm3_Matrix const& m1,
00495 double c2, dvm3_Matrix const& m2 );
00498
00512 void mvmult( dvm3_Vector& result, dvm3_Vector const& v ) const;
00513
00523 void mvmult( double result[], double const v[] ) const;
00524
00534 void mtvmult( dvm3_Vector& result, dvm3_Vector const& v ) const;
00535
00545 void mtvmult( double result[], double const v[] ) const;
00548
00562 void mmult( dvm3_Matrix const& m1, dvm3_Matrix const& m2 );
00565
00580 friend void
00581 dyad_product( dvm3_Matrix& result,
00582 dvm3_Vector const& v1, dvm3_Vector const& v2 );
00583
00594 friend void
00595 dyad_product( dvm3_Matrix& result,
00596 double const v1[], double const v2[] );
00599
00616 friend void
00617 lincomb( dvm3_Matrix& result, double c1, dvm3_Matrix const& m1,
00618 double c2, dvm3_Matrix const& m2 );
00619
00620
00635 friend void
00636 mvmult( dvm3_Vector& result, dvm3_Matrix const& m, dvm3_Vector const& v );
00637
00648 friend void
00649 mvmult( double result[], dvm3_Matrix const& m, double const v[] );
00650
00661 friend void
00662 mtvmult( dvm3_Vector& result, dvm3_Matrix const& m, dvm3_Vector const& v );
00663
00674 friend void
00675 mtvmult( double result[], dvm3_Matrix const& m, double const v[] );
00678
00693 friend void
00694 mmult( dvm3_Matrix& result, dvm3_Matrix const& m1, dvm3_Matrix const& m2 );
00697
00710 friend
00711 dvm3_Matrix operator+ (dvm3_Matrix const& m1, dvm3_Matrix const& m2);
00712
00721 friend
00722 dvm3_Matrix operator- (dvm3_Matrix const& m1, dvm3_Matrix const& m2);
00723
00732 friend
00733 dvm3_Matrix operator* (dvm3_Matrix const& m1, dvm3_Matrix const& m2);
00734
00735
00736
00745 friend
00746 dvm3_Matrix operator/ (dvm3_Matrix const& m1, dvm3_Matrix const& m2);
00747
00760 friend dvm3_Matrix operator+ (double d, dvm3_Matrix const& m);
00761
00770 friend dvm3_Matrix operator- (double d, dvm3_Matrix const& m);
00771
00780 friend dvm3_Matrix operator* (double d, dvm3_Matrix const& m);
00781
00790 friend dvm3_Matrix operator/ (double d, dvm3_Matrix const& m);
00791
00800 friend dvm3_Matrix operator+ (dvm3_Matrix const& m, double d);
00801
00810 friend dvm3_Matrix operator- (dvm3_Matrix const& m, double d);
00811
00820 friend dvm3_Matrix operator* (dvm3_Matrix const& m, double d);
00821
00830 friend dvm3_Matrix operator/ (dvm3_Matrix const& m, double d);
00833
00849 void orthonormalize();
00852
00864 std::ostream& print_on( std::ostream& os, char const pre[] = "",
00865 char const post[] = "") const;
00866
00872 friend std::ostream& operator<<( std::ostream& os, dvm3_Matrix const& );
00873
00882 void
00883 cprint_on( FILE* os,
00884 char const prefix[] = "", char const postfix[] = "" );
00886 };
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901 inline dvm3_Matrix::
00902 ~dvm3_Matrix()
00903 {}
00904
00905 inline dvm3_Matrix::
00906 dvm3_Matrix()
00907 {}
00908
00909 inline dvm3_Matrix::
00910 dvm3_Matrix( dvm3_Matrix const& other )
00911 { data_ = other.data_; }
00912
00913 inline dvm3_Matrix::
00914 dvm3_Matrix( dvm3_Vector const& x,
00915 dvm3_Vector const& y,
00916 dvm3_Vector const& z )
00917 {
00918 vm_V3Math<double>::copy( &data_.m_[ERow0], x.data_.x_ );
00919 vm_V3Math<double>::copy( &data_.m_[ERow1], y.data_.x_ );
00920 vm_V3Math<double>::copy( &data_.m_[ERow2], z.data_.x_ );
00921 }
00922
00923 inline dvm3_Matrix::
00924 dvm3_Matrix( double const x[],
00925 double const y[],
00926 double const z[] )
00927 {
00928 vm_V3Math<double>::copy( &data_.m_[ERow0], x );
00929 vm_V3Math<double>::copy( &data_.m_[ERow1], y );
00930 vm_V3Math<double>::copy( &data_.m_[ERow2], z );
00931 }
00932
00933 inline void dvm3_Matrix::
00934 init_by_row( dvm3_Vector const& x,
00935 dvm3_Vector const& y,
00936 dvm3_Vector const& z)
00937 { vm_M3Math<double>::init_by_row( data_.m_,
00938 x.data_.x_, y.data_.x_, z.data_.x_ ); }
00939
00940 inline void dvm3_Matrix::
00941 init_by_row( double const x[],
00942 double const y[],
00943 double const z[])
00944 { vm_M3Math<double>::init_by_row( data_.m_, x, y, z ); }
00945
00946 inline void dvm3_Matrix::
00947 init_by_col( dvm3_Vector const& x,
00948 dvm3_Vector const& y,
00949 dvm3_Vector const& z)
00950 { vm_M3Math<double>::init_by_col( data_.m_,
00951 x.data_.x_, y.data_.x_, z.data_.x_ ); }
00952
00953 inline void dvm3_Matrix::
00954 init_by_col( double const x[],
00955 double const y[],
00956 double const z[])
00957 { vm_M3Math<double>::init_by_col( data_.m_, x, y, z ); }
00958
00959 inline dvm3_Matrix& dvm3_Matrix::
00960 operator=( dvm3_Matrix const& m )
00961 {
00962 vm_M3Math<double>::copy( data_.m_, m.data_.m_ );
00963 return *this;
00964 }
00965
00966 inline dvm3_Matrix& dvm3_Matrix::
00967 operator=( double rhs )
00968 {
00969 vm_M3Math<double>::set( data_.m_, rhs );
00970 return *this;
00971 }
00972
00973 inline void dvm3_Matrix::
00974 inject_row( int whichrow, double const row[] )
00975 { vm_M3Math<double>::inject_row( data_.m_, row, whichrow ); }
00976
00977 inline void dvm3_Matrix::
00978 inject_row( int whichrow, dvm3_Vector const& row )
00979 { vm_M3Math<double>::inject_row( data_.m_, row.data_.x_, whichrow ); }
00980
00981 inline void dvm3_Matrix::
00982 inject_col( int whichcol, double const col[] )
00983 { vm_M3Math<double>::inject_col( data_.m_, col, whichcol ); }
00984
00985 inline void dvm3_Matrix::
00986 inject_col( int whichcol, dvm3_Vector const& col )
00987 { vm_M3Math<double>::inject_col( data_.m_, col.data_.x_, whichcol ); }
00988
00989 inline void dvm3_Matrix::
00990 extract_row( int whichrow, double row[] ) const
00991 { vm_M3Math<double>::extract_row( data_.m_, row, whichrow ); }
00992
00993 inline void dvm3_Matrix::
00994 extract_row( int whichrow, dvm3_Vector& row ) const
00995 { vm_M3Math<double>::extract_row( data_.m_, row.data_.x_, whichrow ); }
00996
00997 inline void dvm3_Matrix::
00998 extract_col( int whichcol, double col[] ) const
00999 { vm_M3Math<double>::extract_col( data_.m_, col, whichcol ); }
01000
01001 inline void dvm3_Matrix::
01002 extract_col( int whichcol, dvm3_Vector& col ) const
01003 { vm_M3Math<double>::extract_col( data_.m_, col.data_.x_, whichcol ); }
01004
01005 inline double const* dvm3_Matrix::
01006 operator[](int i) const
01007 { return &data_.m_[EColStride*i]; }
01008
01009 inline double* dvm3_Matrix::
01010 operator[](int i)
01011 { return &data_.m_[EColStride*i]; }
01012
01013 inline double dvm3_Matrix::
01014 operator()(int i, int j) const
01015 { return *( data_.m_ + EColStride*i + j ); }
01016
01017 inline double& dvm3_Matrix::
01018 operator()(int i, int j)
01019 { return *( data_.m_ + EColStride*i + j ); }
01020
01021 inline dvm3_Matrix& dvm3_Matrix::operator+()
01022 { return *this; }
01023
01024 inline dvm3_Matrix& dvm3_Matrix::
01025 operator-()
01026 { vm_M3Math<double>::negate( data_.m_ ); return *this; }
01027
01028 #define _tpl_OpEQ_(_OPEQ_,_OPNAM_) \
01029 inline dvm3_Matrix& dvm3_Matrix::operator _OPEQ_ ( dvm3_Matrix const& m ) \
01030 { vm_M3Math<double>::_OPNAM_( data_.m_, m.data_.m_ ); return *this; } \
01031 inline dvm3_Matrix& dvm3_Matrix::operator _OPEQ_ ( double r ) \
01032 { vm_M3Math<double>::_OPNAM_( data_.m_, r ); return *this; }
01033 _tpl_OpEQ_(+=,add_eq)
01034 _tpl_OpEQ_(-=,sub_eq)
01035 _tpl_OpEQ_(*=,mul_eq)
01036 _tpl_OpEQ_(/=,div_eq)
01037 #undef _tpl_OpEQ_
01038
01039 #define _tpl_Op_(_OP_,_OPNAM_) \
01040 inline dvm3_Matrix operator _OP_ ( dvm3_Matrix const& m1, \
01041 dvm3_Matrix const& m2 ) \
01042 { dvm3_Matrix res(m1); \
01043 vm_M3Math<double>::_OPNAM_( res.data_.m_, m1.data_.m_, m2.data_.m_ ); \
01044 return res; } \
01045 inline dvm3_Matrix operator _OP_ ( dvm3_Matrix const& m, double r ) \
01046 { dvm3_Matrix res(m); \
01047 vm_M3Math<double>::_OPNAM_( res.data_.m_, m.data_.m_, r ); \
01048 return res; } \
01049 inline dvm3_Matrix operator _OP_ ( double r, dvm3_Matrix const& m ) \
01050 { dvm3_Matrix res(m); \
01051 vm_M3Math<double>::_OPNAM_( res.data_.m_, r, m.data_.m_ ); \
01052 return res; }
01053 _tpl_Op_(+,add)
01054 _tpl_Op_(-,sub)
01055 _tpl_Op_(*,mul)
01056 _tpl_Op_(/,div)
01057 #undef _tpl_Op_
01058
01059 inline void dvm3_Matrix::
01060 dyad_product( dvm3_Vector const& v1, dvm3_Vector const& v2 )
01061 { vm_M3Math<double>::dyad_product( data_.m_, v1.data_.x_, v2.data_.x_ ); }
01062
01063 inline void dvm3_Matrix::
01064 dyad_product( double const v1[], double const v2[] )
01065 { vm_M3Math<double>::dyad_product( data_.m_, v1, v2 ); }
01066
01067 inline void dvm3_Matrix::
01068 lincomb( double c1, dvm3_Matrix const& m1,
01069 double c2, dvm3_Matrix const& m2 )
01070 { vm_M3Math<double>::lincomb( data_.m_, c1, m1.data_.m_, c2, m2.data_.m_ ); }
01071
01072 inline void dvm3_Matrix::
01073 mvmult( dvm3_Vector& result, dvm3_Vector const& v ) const
01074 { vm_M3Math<double>::mvmult( result.data_.x_, data_.m_, v.data_.x_ ); }
01075
01076 inline void dvm3_Matrix::
01077 mvmult( double result[], double const v[] ) const
01078 { vm_M3Math<double>::mvmult( result, data_.m_, v ); }
01079
01080 inline void dvm3_Matrix::
01081 mtvmult( dvm3_Vector& result, dvm3_Vector const& v ) const
01082 { vm_M3Math<double>::mtvmult( result.data_.x_, data_.m_, v.data_.x_ ); }
01083
01084 inline void dvm3_Matrix::
01085 mtvmult( double result[], double const v[] ) const
01086 { vm_M3Math<double>::mtvmult( result, data_.m_, v ); }
01087
01088 inline void dvm3_Matrix::
01089 mmult( dvm3_Matrix const& m1, dvm3_Matrix const& m2 )
01090 { vm_M3Math<double>::mmult( data_.m_, m1.data_.m_, m2.data_.m_ ); }
01091
01092 inline void
01093 dyad_product( dvm3_Matrix& result,
01094 dvm3_Vector const& v1, dvm3_Vector const& v2 )
01095 { vm_M3Math<double>::dyad_product( result.data_.m_,
01096 v1.data_.x_, v2.data_.x_ ); }
01097
01098 inline void
01099 dyad_product( dvm3_Matrix& result,
01100 double const v1[], double const v2[] )
01101 { vm_M3Math<double>::dyad_product( result.data_.m_, v1, v2 ); }
01102
01103 inline void
01104 lincomb( dvm3_Matrix& result, double c1, dvm3_Matrix const& m1,
01105 double c2, dvm3_Matrix const& m2 )
01106 { vm_M3Math<double>::lincomb( result.data_.m_,
01107 c1, m1.data_.m_, c2, m2.data_.m_ ); }
01108
01109 inline void
01110 mvmult( dvm3_Vector& result, dvm3_Matrix const& m, dvm3_Vector const& v )
01111 { m.mvmult( result.data_.x_, v.data_.x_ ); }
01112
01113 inline void
01114 mvmult( double result[], dvm3_Matrix const& m, double const v[] )
01115 { m.mvmult( result, v ); }
01116
01117 inline void
01118 mtvmult( dvm3_Vector& result, dvm3_Matrix const& m, dvm3_Vector const& v )
01119 { m.mtvmult( result.data_.x_, v.data_.x_ ); }
01120
01121 inline void
01122 mtvmult( double result[], dvm3_Matrix const& m, double const v[] )
01123 { m.mtvmult( result, v ); }
01124
01125 inline void
01126 mmult( dvm3_Matrix& result, dvm3_Matrix const& m1, dvm3_Matrix const& m2 )
01127 { result.mmult( m1, m2 ); }
01128
01129 inline std::ostream& dvm3_Matrix::
01130 print_on( std::ostream& os, char const pre[], char const post[] ) const
01131 { return vm_M3Math<double>::print_on( os, data_.m_, pre, post ); }
01132
01133 inline std::ostream&
01134 operator<<( std::ostream& os, const dvm3_Matrix& m )
01135 { return m.print_on( os, "[", "]" ); }
01136
01137 inline void dvm3_Matrix::
01138 cprint_on( FILE* of, char const pre[], char const post[] )
01139 { vm_M3Math<double>::cprint_on( of, data_.m_, pre, post ); }
01140
01141 #endif