00001
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
00037
00038
00039
00040
00041
00042
00043
00044
00045 #ifndef CLIPPER_TYPES
00046 #define CLIPPER_TYPES
00047
00048
00049 #include "clipper_util.h"
00050 #include "clipper_memory.h"
00051
00052
00053 namespace clipper
00054 {
00055
00056 template<class T> class Mat33sym;
00057
00058
00060
00064 class String : public std::string
00065 {
00066 public:
00067 inline String() : std::string() {}
00068 inline String( const std::string str ) : std::string( str ) {}
00069 inline String( const char* str ) : std::string( str ) {}
00070 String( const char* str, const int l );
00071 String( const int i, const int w = 4 );
00072 String( const long i, const int w = 4 );
00073
00074 String( const float f, const int w = 6, const int p = 6 );
00076 String( const double f, const int w = 6, const int p = 6 );
00077
00079 std::vector<String> split(const String sep) const;
00081 String trim() const;
00082
00084 String tail() const;
00086 String head() const;
00088 String nohead() const;
00090 String notail() const;
00091
00093 static String rational( const double f, const int b, const bool sign=false );
00094
00095 int i() const;
00096 long l() const;
00097 ftype32 f32() const;
00098 ftype64 f64() const;
00099 ftype f() const;
00100 ftype rational() const;
00101 };
00102
00103
00105 template<class T = ftype> class Vec3
00106 {
00107 public:
00109 inline Vec3() {}
00111 inline Vec3( const T& v0, const T& v1, const T& v2 )
00112 { vec[0] = v0; vec[1] = v1; vec[2] = v2; }
00114 template<class TT> explicit Vec3( const Vec3<TT>& v )
00115 { vec[0] = TT(v[0]); vec[1] = TT(v[1]); vec[2] = TT(v[2]); }
00117 bool equals( const Vec3<T>& v, const T& tol ) const;
00119 inline const T& operator []( const int& i ) const { return vec[i]; }
00121 inline T& operator []( const int& i ) { return vec[i]; }
00123 inline Vec3<T> unit() const
00124 { return (*this)*T(1.0/sqrt(ftype(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]))); }
00126 inline static Vec3<T> zero() { return Vec3<T>( 0, 0, 0 ); }
00128 inline static Vec3<T> null() { return Vec3<T>( T(Util::nan()), 0, 0 ); }
00130 inline bool is_null() const { return Util::is_nan( vec[0] ); }
00132 inline static T dot( const Vec3<T>& v1, const Vec3<T>& v2 )
00133 { return (v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]); }
00135 inline static Vec3<T> cross( const Vec3<T>& v1, const Vec3<T>& v2 )
00136 { return Vec3<T>(v1[1]*v2[2]-v2[1]*v1[2],
00137 v1[2]*v2[0]-v2[2]*v1[0],
00138 v1[0]*v2[1]-v2[0]*v1[1]); }
00140 String format() const
00141 { return "("+String(vec[0],10,4)+","+String(vec[1],10,4)+","+String(vec[2],10,4)+")"; }
00143 inline const Vec3<T>& operator += ( const Vec3<T>& v )
00144 { vec[0] += v[0]; vec[1] += v[1]; vec[2] += v[2]; return (*this); }
00146 inline const Vec3<T>& operator -= ( const Vec3<T>& v )
00147 { vec[0] -= v[0]; vec[1] -= v[1]; vec[2] -= v[2]; return (*this); }
00149
00151
00153
00155
00157
00159
00161
00163
00164 private:
00165 T vec[3];
00166 };
00167 template<class T> inline int operator == ( const Vec3<T>& v1, const Vec3<T>& v2 ) { return (v1[0]==v2[0] && v1[1]==v2[1] && v1[2]==v2[2]); }
00168 template<class T> inline int operator != ( const Vec3<T>& v1, const Vec3<T>& v2 ) { return (v1[0]!=v2[0] || v1[1]!=v2[1] || v1[2]!=v2[2]); }
00169 template<class T> inline Vec3<T> operator -( const Vec3<T>& v )
00170 { return Vec3<T>( -v[0], -v[1], -v[2] ); }
00171 template<class T> inline Vec3<T> operator +( const Vec3<T>& v1, const Vec3<T> &v2 ) { return Vec3<T>( v1[0]+v2[0], v1[1]+v2[1], v1[2]+v2[2]); }
00172 template<class T> inline Vec3<T> operator -( const Vec3<T>& v1, const Vec3<T>& v2 ) { return Vec3<T>( v1[0]-v2[0], v1[1]-v2[1], v1[2]-v2[2]); }
00173 template<class T> inline Vec3<T> operator *( const T& s, const Vec3<T>& v1 )
00174 { return Vec3<T>( s*v1[0], s*v1[1], s*v1[2]); }
00175 template<class T> inline Vec3<T> operator *( const Vec3<T>& v1, const T& s )
00176 { return Vec3<T>( s*v1[0], s*v1[1], s*v1[2]); }
00177 template<class T> inline T operator *( const Vec3<T>& v1, const Vec3<T>& v2 )
00178 { return Vec3<T>::dot(v1,v2); }
00179
00180
00182 template<class T = ftype> class Mat33
00183 {
00184 public:
00186 inline Mat33() {}
00188 inline Mat33( const T& m00, const T& m01, const T& m02, const T& m10, const T& m11, const T& m12, const T& m20, const T& m21, const T& m22 )
00189 { mat[0][0] = m00; mat[0][1] = m01; mat[0][2] = m02; mat[1][0] = m10; mat[1][1] = m11; mat[1][2] = m12; mat[2][0] = m20; mat[2][1] = m21; mat[2][2] = m22; }
00191 template<class TT> explicit Mat33( const Mat33<TT>& m );
00193 template<class TT> explicit Mat33( const Mat33sym<TT>& m );
00194 T det() const;
00195 Mat33<T> inverse() const;
00196 Mat33<T> transpose() const;
00197 bool equals( const Mat33<T>& m, const T& tol ) const;
00198 inline const T& operator ()( const int& i, const int& j ) const
00199 { return mat[i][j]; }
00200 inline T& operator ()( const int& i, const int& j )
00201 { return mat[i][j]; }
00202
00203 String format() const
00204 { return "|"+String(mat[0][0],10,4)+","+String(mat[0][1],10,4)+","+String(mat[0][2],10,4)+"|\n|"+String(mat[1][0],10,4)+","+String(mat[1][1],10,4)+","+String(mat[1][2],10,4)+"|\n|"+String(mat[2][0],10,4)+","+String(mat[2][1],10,4)+","+String(mat[2][2],10,4)+"|"; }
00206 inline static Mat33<T> identity() { Mat33<T> m; m.mat[0][0] = m.mat[1][1] = m.mat[2][2] = 1; m.mat[0][1] = m.mat[0][2] = m.mat[1][0] = m.mat[1][2] = m.mat[2][0] = m.mat[2][1] = 0; return m; }
00208 inline static Mat33<T> null() { Mat33<T> m; m.mat[0][0] = Util::nan(); return m; }
00210 bool inline is_null() const { return Util::is_nan( mat[0][0] ); }
00212
00213
00215
00217
00219
00221
00223
00224 private:
00225 T mat[3][3];
00226 };
00227 template<class T> inline Vec3<T> operator *( const Mat33<T>& m, const Vec3<T>& v )
00228 { return Vec3<T>( m(0,0)*v[0]+m(0,1)*v[1]+m(0,2)*v[2],
00229 m(1,0)*v[0]+m(1,1)*v[1]+m(1,2)*v[2],
00230 m(2,0)*v[0]+m(2,1)*v[1]+m(2,2)*v[2] ); }
00231 template<class T> inline Vec3<T> operator *( const Vec3<T>& v, const Mat33<T>& m )
00232 { return Vec3<T>( v[0]*m(0,0)+v[1]*m(1,0)+v[2]*m(2,0),
00233 v[0]*m(0,1)+v[1]*m(1,1)+v[2]*m(2,1),
00234 v[0]*m(0,2)+v[1]*m(1,2)+v[2]*m(2,2) ); }
00235 template<class T> inline Mat33<T> operator *( const Mat33<T>& m1, const Mat33<T>& m2 )
00236 {
00237 return Mat33<T> ( m1(0,0)*m2(0,0) + m1(0,1)*m2(1,0) + m1(0,2)*m2(2,0),
00238 m1(0,0)*m2(0,1) + m1(0,1)*m2(1,1) + m1(0,2)*m2(2,1),
00239 m1(0,0)*m2(0,2) + m1(0,1)*m2(1,2) + m1(0,2)*m2(2,2),
00240 m1(1,0)*m2(0,0) + m1(1,1)*m2(1,0) + m1(1,2)*m2(2,0),
00241 m1(1,0)*m2(0,1) + m1(1,1)*m2(1,1) + m1(1,2)*m2(2,1),
00242 m1(1,0)*m2(0,2) + m1(1,1)*m2(1,2) + m1(1,2)*m2(2,2),
00243 m1(2,0)*m2(0,0) + m1(2,1)*m2(1,0) + m1(2,2)*m2(2,0),
00244 m1(2,0)*m2(0,1) + m1(2,1)*m2(1,1) + m1(2,2)*m2(2,1),
00245 m1(2,0)*m2(0,2) + m1(2,1)*m2(1,2) + m1(2,2)*m2(2,2) );
00246 }
00247 template<class T> inline Mat33<T> operator +( const Mat33<T>& m1, const Mat33<T>& m2 )
00248 { return Mat33<T>( m1(0,0)+m2(0,0), m1(0,1)+m2(0,1), m1(0,2)+m2(0,2),
00249 m1(1,0)+m2(1,0), m1(1,1)+m2(1,1), m1(1,2)+m2(1,2),
00250 m1(2,0)+m2(2,0), m1(2,1)+m2(2,1), m1(2,2)+m2(2,2) ); }
00251 template<class T> inline Mat33<T> operator -( const Mat33<T>& m )
00252 { return Mat33<T>( -m(0,0), -m(0,1), -m(0,2),
00253 -m(1,0), -m(1,1), -m(1,2),
00254 -m(2,0), -m(2,1), -m(2,2) ); }
00255
00256
00258 template<class T = ftype> class Mat33sym
00259 {
00260 public:
00262 inline Mat33sym() {}
00264 template<class TT> explicit Mat33sym( const Mat33<TT>& m ) :
00265 m00(m(0,0)), m11(m(1,1)), m22(m(2,2)),
00266 m01(m(0,1)), m02(m(0,2)), m12(m(1,2)) {}
00268 template<class TT> explicit Mat33sym( const Mat33sym<TT>& m ) :
00269 m00(m.mat00()), m11(m.mat11()), m22(m.mat22()),
00270 m01(m.mat01()), m02(m.mat02()), m12(m.mat12()) {}
00272 inline Mat33sym( const T& c00, const T& c11, const T& c22,
00273 const T& c01, const T& c02, const T& c12 ) :
00274 m00(c00), m11(c11), m22(c22), m01(c01), m02(c02), m12(c12) {}
00276 String format() const
00277 { return "|"+String(m00,10,4)+","+String(m01,10,4)+","+String(m02,10,4)+"|\n|"+String(m01,10,4)+","+String(m11,10,4)+","+String(m12,10,4)+"|\n|"+String(m02,10,4)+","+String(m12,10,4)+","+String(m22,10,4)+"|"; }
00279 inline static Mat33sym<T> identity()
00280 { return Mat33sym<T>( 1, 1, 1, 0, 0, 0 ); }
00282 inline static Mat33sym<T> null()
00283 { return Mat33sym<T>(Util::nan(),0,0,0,0,0); }
00285 inline bool is_null() const { return Util::is_nan( m00 ); }
00287 T quad_form( const Vec3<T>& v ) const;
00288 T det() const;
00289 Mat33<T> sqrt() const;
00290 Mat33sym<T> inverse() const;
00291 inline const T& mat00() const { return m00; }
00292 inline const T& mat11() const { return m11; }
00293 inline const T& mat22() const { return m22; }
00294 inline const T& mat01() const { return m01; }
00295 inline const T& mat02() const { return m02; }
00296 inline const T& mat12() const { return m12; }
00297
00298 const T& operator ()( const int& i, const int& j ) const;
00300
00302
00304
00305 private:
00306 T m00, m11, m22, m01, m02, m12;
00307 };
00308 template<class T> inline Vec3<T> operator *( const Mat33sym<T>& m, const Vec3<T>& v )
00309 { return Vec3<T>( m.mat00()*v[0]+m.mat01()*v[1]+m.mat02()*v[2],
00310 m.mat01()*v[0]+m.mat11()*v[1]+m.mat12()*v[2],
00311 m.mat02()*v[0]+m.mat12()*v[1]+m.mat22()*v[2] ); }
00312 template<class T> inline Mat33sym<T> operator +( const Mat33sym<T>& m1, const Mat33sym<T>& m2 )
00313 { return Mat33sym<T>( m1.mat00()+m2.mat00(), m1.mat11()+m2.mat11(),
00314 m1.mat22()+m2.mat22(), m1.mat01()+m2.mat01(),
00315 m1.mat02()+m2.mat02(), m1.mat12()+m2.mat12() ); }
00316 template<class T> inline Mat33sym<T> operator -( const Mat33sym<T>& m )
00317 { return Mat33sym<T>( -m.mat00(), -m.mat11(), -m.mat22(),
00318 -m.mat01(), -m.mat02(), -m.mat12() ); }
00319
00320
00322 template<class T = ftype> class RTop
00323 {
00324 public:
00326 inline RTop() {}
00328 inline explicit RTop( const Mat33<T>& r ) : rot_( r ), trn_( Vec3<T>::zero() ) {}
00330 inline RTop( const Mat33<T>& r, const Vec3<T>& t ) : rot_( r ), trn_( t ) {}
00332 RTop<T> inverse() const
00333 { Mat33<T> minv = rot().inverse(); return RTop<T>(minv, -(minv*trn())); }
00335 inline bool equals( const RTop<T>& m, const T& tol ) const
00336 { return ( rot().equals(m.rot(),tol) && trn().equals(m.trn(),tol) ); }
00337 inline const Mat33<T>& rot() const { return rot_; }
00338 inline const Vec3<T>& trn() const { return trn_; }
00339 inline Mat33<T>& rot() { return rot_; }
00340 inline Vec3<T>& trn() { return trn_; }
00341
00342 inline static RTop<T> identity()
00343 { return RTop<T>( Mat33<T>::identity(), Vec3<T>::zero() ); }
00345 inline static RTop<T> null()
00346 { return RTop<T>( Mat33<T>::null(), Vec3<T>::null() ); }
00348 inline bool is_null() const { return rot_.is_null() || trn_.is_null(); }
00350 String format() const { return rot_.format() + "\n" + trn_.format(); }
00352
00354
00355 private:
00356 Mat33<T> rot_; Vec3<T> trn_;
00357 };
00358 template<class T> inline Vec3<T> operator *( const RTop<T>& r, const Vec3<T>& v ) { return r.rot()*v + r.trn(); }
00359 template<class T> inline RTop<T> operator *( const RTop<T>& r1, const RTop<T>& r2 ) { return RTop<T>( r1.rot()*r2.rot(), r1.rot()*r2.trn()+r1.trn() ); }
00360
00361
00362
00364 template<class T = ftype> class Array2d
00365 {
00366 public:
00368 inline Array2d() { d1_ = d2_ = 0; }
00370 inline Array2d( const int& d1, const int& d2 ) { resize( d1, d2 ); }
00372 inline Array2d( const int& d1, const int& d2, T val )
00373 { resize( d1, d2, val ); }
00375 void inline resize( const int& d1, const int& d2 )
00376 { d1_ = d1; d2_ = d2; data.resize( size() ); }
00378 void inline resize( const int& d1, const int& d2, const T& val )
00379 { d1_ = d1; d2_ = d2; data.resize( size(), val ); }
00380 inline int size() const { return d1_ * d2_; }
00381 inline const int& rows() const { return d1_; }
00382 inline const int& cols() const { return d2_; }
00383
00384 inline const T& operator () ( const int& i1, const int& i2 ) const
00385 { return data[ i1*d2_ + i2 ]; }
00387 inline T& operator () ( const int& i1, const int& i2 )
00388 { return data[ i1*d2_ + i2 ]; }
00389 protected:
00390 std::vector<T> data;
00391 int d1_, d2_;
00392 };
00393
00394
00396 template<class T = ftype> class Matrix : public Array2d<T>
00397 {
00398 public:
00400 inline Matrix() {}
00402 inline Matrix( const int& d1, const int& d2 )
00403 { Array2d<T>::resize( d1, d2 ); }
00405 inline Matrix( const int& d1, const int& d2, T val )
00406 { Array2d<T>::resize( d1, d2, val ); }
00408 std::vector<T> solve( const std::vector<T>& b ) const;
00410 std::vector<T> eigen( const bool sort = true );
00412
00413
00414 };
00415 template<class T> std::vector<T> operator *( const Matrix<T>& m, const std::vector<T>& v )
00416 {
00417 if ( m.cols() != v.size() )
00418 Message::message( Message_fatal( "Matrix*vector dimension mismatch" ) );
00419 std::vector<T> r( m.rows() );
00420 int i, j; T s;
00421 for ( i = 0; i < m.rows(); i++ ) {
00422 s = T(0);
00423 for ( j = 0; j < m.cols(); j++ ) s += m(i,j) * v[j];
00424 r[i] = s;
00425 }
00426 return r;
00427 }
00428
00429
00430
00431
00432
00433 template<class T> bool Vec3<T>::equals( const Vec3<T>& v, const T& tol ) const
00434 {
00435 return ( pow(vec[0]-v[0],T(2)) + pow(vec[1]-v[1],T(2)) +
00436 pow(vec[2]-v[2],T(2)) <= pow(tol,T(2)) );
00437 }
00438
00439 template<class T> template<class TT> Mat33<T>::Mat33( const Mat33<TT>& m )
00440 {
00441 mat[0][0]=T(m(0,0)); mat[0][1]=T(m(0,1)); mat[0][2]=T(m(0,2));
00442 mat[1][0]=T(m(1,0)); mat[1][1]=T(m(1,1)); mat[1][2]=T(m(1,2));
00443 mat[2][0]=T(m(2,0)); mat[2][1]=T(m(2,1)); mat[2][2]=T(m(2,2));
00444 }
00445
00446 template<class T> template<class TT> Mat33<T>::Mat33( const Mat33sym<TT>& m )
00447 {
00448 mat[0][0]=T(m.mat00());
00449 mat[1][1]=T(m.mat11());
00450 mat[2][2]=T(m.mat22());
00451 mat[0][1]=mat[1][0]=T(m.mat01());
00452 mat[0][2]=mat[2][0]=T(m.mat02());
00453 mat[1][2]=mat[2][1]=T(m.mat12());
00454 }
00455
00456 template<class T> bool Mat33<T>::equals( const Mat33<T>& m, const T& tol ) const
00457 {
00458 return ( pow(mat[0][0]-m(0,0),T(2)) + pow(mat[0][1]-m(0,1),T(2)) +
00459 pow(mat[0][2]-m(0,2),T(2)) + pow(mat[1][0]-m(1,0),T(2)) +
00460 pow(mat[1][1]-m(1,1),T(2)) + pow(mat[1][2]-m(1,2),T(2)) +
00461 pow(mat[2][0]-m(2,0),T(2)) + pow(mat[2][1]-m(2,1),T(2)) +
00462 pow(mat[2][2]-m(2,2),T(2)) <= pow(tol,T(2)) );
00463 }
00464
00465 template<class T> T Mat33<T>::det() const
00466 {
00467 return ( mat[0][0]*(mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1]) +
00468 mat[0][1]*(mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2]) +
00469 mat[0][2]*(mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0]) );
00470 }
00471
00472 template<class T> Mat33<T> Mat33<T>::inverse() const
00473 {
00474 T d = det();
00475 Mat33<T> inv;
00476 inv(0,0) = ( mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1] ) / d;
00477 inv(1,0) = ( mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2] ) / d;
00478 inv(2,0) = ( mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0] ) / d;
00479 inv(0,1) = ( mat[2][1]*mat[0][2] - mat[2][2]*mat[0][1] ) / d;
00480 inv(1,1) = ( mat[2][2]*mat[0][0] - mat[2][0]*mat[0][2] ) / d;
00481 inv(2,1) = ( mat[2][0]*mat[0][1] - mat[2][1]*mat[0][0] ) / d;
00482 inv(0,2) = ( mat[0][1]*mat[1][2] - mat[0][2]*mat[1][1] ) / d;
00483 inv(1,2) = ( mat[0][2]*mat[1][0] - mat[0][0]*mat[1][2] ) / d;
00484 inv(2,2) = ( mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0] ) / d;
00485 return inv;
00486 }
00487
00488 template<class T> Mat33<T> Mat33<T>::transpose() const
00489 {
00490 Mat33<T> t;
00491 t(0,0) = mat[0][0]; t(0,1) = mat[1][0]; t(0,2) = mat[2][0];
00492 t(1,0) = mat[0][1]; t(1,1) = mat[1][1]; t(1,2) = mat[2][1];
00493 t(2,0) = mat[0][2]; t(2,1) = mat[1][2]; t(2,2) = mat[2][2];
00494 return t;
00495 }
00496
00497 template<class T> T Mat33sym<T>::det() const
00498 {
00499 return ( m00*(m11*m22 - m12*m12) + m01*(m12*m02 - m01*m22) +
00500 m02*(m01*m12 - m11*m02) );
00501 }
00502
00503 template<class T> Mat33<T> Mat33sym<T>::sqrt() const
00504 {
00505 Mat33<T> half( Mat33sym<T>( 0.5, 0.5, 0.5, 0.0, 0.0, 0.0 ) );
00506 Mat33<T> target( *this );
00507 Mat33<T> result( target );
00508 result(1,0) = result(2,0) = result(2,1) = 0.0;
00509 for ( int i = 0; i < 10; i++ )
00510 result = half * ( result.inverse() * target + result );
00511 return result;
00512 }
00513
00514 template<class T> Mat33sym<T> Mat33sym<T>::inverse() const
00515 {
00516 T d = det();
00517 return Mat33sym<T> ( ( m11*m22 - m12*m12 ) / d,
00518 ( m22*m00 - m02*m02 ) / d,
00519 ( m00*m11 - m01*m01 ) / d,
00520 ( m12*m02 - m22*m01 ) / d,
00521 ( m01*m12 - m02*m11 ) / d,
00522 ( m02*m01 - m00*m12 ) / d );
00523 }
00524
00525 template<class T> T Mat33sym<T>::quad_form( const Vec3<T>& v ) const
00526 {
00527 return ( v[0]*( v[0]*m00 + 2*(v[1]*m01+v[2]*m02) ) +
00528 v[1]*( v[1]*m11 + 2*(v[2]*m12) ) + v[2]*v[2]*m22 );
00529 }
00530
00531 template<class T> const T& Mat33sym<T>::operator ()( const int& i, const int& j ) const
00532 {
00533 switch (i) {
00534 case 0:
00535 switch (j) {
00536 case 0: return m00;
00537 case 1: return m01;
00538 default: return m02;
00539 }
00540 case 1:
00541 switch (j) {
00542 case 0: return m01;
00543 case 1: return m11;
00544 default: return m12;
00545 }
00546 default:
00547 switch (j) {
00548 case 0: return m02;
00549 case 1: return m12;
00550 default: return m22;
00551 }
00552 }
00553 }
00554
00555
00556
00557
00558
00561 template<class T> std::vector<T> Matrix<T>::solve( const std::vector<T>& b ) const
00562 {
00563 if ( Array2d<T>::rows() != Array2d<T>::cols() )
00564 Message::message( Message_fatal("Matrix.solve() matrix not square") );
00565 if ( Array2d<T>::rows() != b.size() )
00566 Message::message( Message_fatal("Matrix.solve() matrix/vector mismatch") );
00567 const int n = Array2d<T>::rows();
00568
00569
00570 T s, pivot;
00571 int i, j, k;
00572
00573 Matrix<T> a = *this;
00574 std::vector<T> x = b;
00575 for ( i = 0; i < n; i++ ) {
00576
00577 j = i;
00578 for ( k = i+1; k < n; k++ )
00579 if ( fabs(a(k,i)) > fabs(a(j,i)) ) j = k;
00580
00581 for ( k = 0; k < n; k++ )
00582 Util::swap( a(i,k), a(j,k) );
00583 Util::swap( x[i], x[j] );
00584
00585 pivot = a(i,i);
00586 for ( j = 0; j < n; j++ ) {
00587 if ( j != i ) {
00588 s = a(j,i) / pivot;
00589 for ( k = i+1; k < n; k++ ) a(j,k) = a(j,k) - s*a(i,k);
00590 x[j] = x[j] - s*x[i];
00591 }
00592 }
00593 }
00594 for ( i = 0; i < n; i++ ) x[i] /= a(i,i);
00595 return x;
00596 }
00597
00598
00604 template<class T> std::vector<T> Matrix<T>::eigen( const bool sort )
00605 {
00606 if ( Array2d<T>::rows() != Array2d<T>::cols() )
00607 Message::message( Message_fatal( "Matrix.eigen() matrix not square" ) );
00608 const int n = Array2d<T>::rows();
00609
00610 int cyc, j, q, p;
00611 T spp, spq, t, s, c, theta, tau, h, ap, aq, a_pq;
00612
00613 Matrix<T>& mat = *this;
00614 Matrix evec( n, n, 0.0 );
00615 std::vector<T> eval( n );
00616 std::vector<T> b( n );
00617 std::vector<T> z( n );
00618
00619
00620 for( p = 0; p < n; p++ ) {
00621 evec(p,p) = 1.0;
00622 eval[p] = b[p] = mat(p,p);
00623 }
00624
00625 for ( cyc = 1; cyc <= 50; cyc++ ) {
00626
00627
00628 spp = spq = 0.0;
00629 for ( p=0; p<n-1; p++ ) {
00630 for ( q=p+1; q<n; q++ )
00631 spq += fabs( mat(p,q) );
00632 spp += fabs( mat(p,p) );
00633 }
00634 if ( spq <= 1.0e-12 * spp ) break;
00635
00636
00637 for ( p = 0; p < n; p++ ) z[p] = 0.0;
00638
00639
00640 for( p=0; p<n-1; p++ ) {
00641 for( q=p+1; q<n; q++ ) {
00642 a_pq = mat( p, q );
00643 h = eval[q] - eval[p];
00644 if ( fabs(a_pq) > 1.0e-12*fabs(h) ) {
00645 theta = 0.5*h/a_pq;
00646 t = 1.0/(fabs(theta) + sqrt(1.0 + theta*theta));
00647 if ( theta < 0.0 ) t = -t;
00648 } else {
00649 t = a_pq/h;
00650 }
00651
00652
00653 c = 1.0/sqrt(1.0+t*t);
00654 s = t*c;
00655 tau = s/(1.0+c);
00656 h = t * a_pq;
00657
00658
00659 z[p] -= h;
00660 z[q] += h;
00661 eval[p] -= h;
00662 eval[q] += h;
00663
00664
00665 mat( p, q ) = 0.0;
00666 for ( j = 0; j < p; j++ ) {
00667 ap = mat( j, p );
00668 aq = mat( j, q );
00669 mat( j, p ) = ap - s * ( aq + ap * tau );
00670 mat( j, q ) = aq + s * ( ap - aq * tau );
00671 }
00672 for ( j = p+1; j < q; j++ ) {
00673 ap = mat( p, j );
00674 aq = mat( j, q );
00675 mat( p, j ) = ap - s * ( aq + ap * tau );
00676 mat( j, q ) = aq + s * ( ap - aq * tau );
00677 }
00678 for ( j = q+1; j < n; j++ ) {
00679 ap = mat( p, j );
00680 aq = mat( q, j );
00681 mat( p, j ) = ap - s * ( aq + ap * tau );
00682 mat( q, j ) = aq + s * ( ap - aq * tau );
00683 }
00684
00685 for ( j = 0; j < n; j++ ) {
00686 ap = evec( j, p );
00687 aq = evec( j, q );
00688 evec( j, p ) = ap - s * ( aq + ap * tau );
00689 evec( j, q ) = aq + s * ( ap - aq * tau );
00690 }
00691 }
00692 }
00693
00694 for ( p = 0; p < n; p++ ) {
00695 b[p] += z[p];
00696 eval[p] = b[p];
00697 }
00698 }
00699
00700
00701 if ( sort ) {
00702 for ( p = 0; p < n; p++ ) {
00703 j = p;
00704 for ( q = p+1; q < n; q++ )
00705 if ( eval[q] < eval[j] ) j = q;
00706 Util::swap( eval[p], eval[j] );
00707 for ( q = 0; q < n; q++ )
00708 Util::swap( evec( q, p ), evec( q, j ) );
00709 }
00710 }
00711
00712
00713 mat = evec;
00714 return eval;
00715 }
00716
00717 }
00718
00719 #endif