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
00222 private:
00223 T mat[3][3];
00224 };
00225 template<class T> inline Vec3<T> operator *( const Mat33<T> m, const Vec3<T> v )
00226 { return Vec3<T>( m(0,0)*v[0]+m(0,1)*v[1]+m(0,2)*v[2],
00227 m(1,0)*v[0]+m(1,1)*v[1]+m(1,2)*v[2],
00228 m(2,0)*v[0]+m(2,1)*v[1]+m(2,2)*v[2] ); }
00229 template<class T> inline Vec3<T> operator *( const Vec3<T> v, const Mat33<T> m )
00230 { return Vec3<T>( v[0]*m(0,0)+v[1]*m(1,0)+v[2]*m(2,0),
00231 v[0]*m(0,1)+v[1]*m(1,1)+v[2]*m(2,1),
00232 v[0]*m(0,2)+v[1]*m(1,2)+v[2]*m(2,2) ); }
00233 template<class T> inline Mat33<T> operator *(const Mat33<T>& m1, const Mat33<T>& m2)
00234 {
00235 return Mat33<T> ( m1(0,0)*m2(0,0) + m1(0,1)*m2(1,0) + m1(0,2)*m2(2,0),
00236 m1(0,0)*m2(0,1) + m1(0,1)*m2(1,1) + m1(0,2)*m2(2,1),
00237 m1(0,0)*m2(0,2) + m1(0,1)*m2(1,2) + m1(0,2)*m2(2,2),
00238 m1(1,0)*m2(0,0) + m1(1,1)*m2(1,0) + m1(1,2)*m2(2,0),
00239 m1(1,0)*m2(0,1) + m1(1,1)*m2(1,1) + m1(1,2)*m2(2,1),
00240 m1(1,0)*m2(0,2) + m1(1,1)*m2(1,2) + m1(1,2)*m2(2,2),
00241 m1(2,0)*m2(0,0) + m1(2,1)*m2(1,0) + m1(2,2)*m2(2,0),
00242 m1(2,0)*m2(0,1) + m1(2,1)*m2(1,1) + m1(2,2)*m2(2,1),
00243 m1(2,0)*m2(0,2) + m1(2,1)*m2(1,2) + m1(2,2)*m2(2,2) );
00244 }
00245 template<class T> inline Mat33<T> operator +(const Mat33<T>& m1, const Mat33<T>& m2)
00246 { return Mat33<T>( m1(0,0) + m2(0,0), m1(0,1) + m2(0,1), m1(0,2) + m2(0,2), m1(1,0) + m2(1,0), m1(1,1) + m2(1,1), m1(1,2) + m2(1,2), m1(2,0) + m2(2,0), m1(2,1) + m2(2,1), m1(2,2) + m2(2,2) ); }
00247
00248
00250 template<class T = ftype> class Mat33sym
00251 {
00252 public:
00254 inline Mat33sym() {}
00256 template<class TT> explicit Mat33sym( const Mat33<TT>& m ) :
00257 m00(m(0,0)), m11(m(1,1)), m22(m(2,2)),
00258 m01(m(0,1)), m02(m(0,2)), m12(m(1,2)) {}
00260 template<class TT> explicit Mat33sym( const Mat33sym<TT>& m ) :
00261 m00(m.mat00()), m11(m.mat11()), m22(m.mat22()),
00262 m01(m.mat01()), m02(m.mat02()), m12(m.mat12()) {}
00264 inline Mat33sym( const T& c00, const T& c11, const T& c22,
00265 const T& c01, const T& c02, const T& c12 ) :
00266 m00(c00), m11(c11), m22(c22), m01(c01), m02(c02), m12(c12) {}
00268 String format() const
00269 { 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)+"|"; }
00271 inline static Mat33sym<T> identity()
00272 { return Mat33sym<T>( 1, 1, 1, 0, 0, 0 ); }
00274 inline static Mat33sym<T> null()
00275 { return Mat33sym<T>(Util::nan(),0,0,0,0,0); }
00277 inline bool is_null() const { return Util::is_nan( m00 ); }
00279 T quad_form( const Vec3<T>& v ) const;
00280 T det() const;
00281 Mat33<T> sqrt() const;
00282 Mat33sym<T> inverse() const;
00283 inline const T& mat00() const { return m00; }
00284 inline const T& mat11() const { return m11; }
00285 inline const T& mat22() const { return m22; }
00286 inline const T& mat01() const { return m01; }
00287 inline const T& mat02() const { return m02; }
00288 inline const T& mat12() const { return m12; }
00289
00290 const T& operator ()( const int& i, const int& j ) const;
00292
00293 private:
00294 T m00, m11, m22, m01, m02, m12;
00295 };
00296 template<class T> inline Mat33sym<T> operator +(const Mat33sym<T>& m1, const Mat33sym<T>& m2) { return Mat33sym<T>( m1.mat00()+m2.mat00(), m1.mat11()+m2.mat11(), m1.mat22()+m2.mat22(), m1.mat01()+m2.mat01(), m1.mat02()+m2.mat02(), m1.mat12()+m2.mat12() ); }
00297
00298
00300 template<class T = ftype> class RTop
00301 {
00302 public:
00304 inline RTop() {}
00306 inline explicit RTop( const Mat33<T>& r ) : rot_( r ), trn_( Vec3<T>::zero() ) {}
00308 inline RTop( const Mat33<T>& r, const Vec3<T>& t ) : rot_( r ), trn_( t ) {}
00310 RTop<T> inverse() const
00311 { Mat33<T> minv = rot().inverse(); return RTop<T>(minv, -(minv*trn())); }
00313 inline bool equals( const RTop<T>& m, const T& tol ) const
00314 { return ( rot().equals(m.rot(),tol) && trn().equals(m.trn(),tol) ); }
00315 inline const Mat33<T>& rot() const { return rot_; }
00316 inline const Vec3<T>& trn() const { return trn_; }
00317 inline Mat33<T>& rot() { return rot_; }
00318 inline Vec3<T>& trn() { return trn_; }
00319
00320 inline static RTop<T> identity()
00321 { return RTop<T>( Mat33<T>::identity(), Vec3<T>::zero() ); }
00323 inline static RTop<T> null()
00324 { return RTop<T>( Mat33<T>::null(), Vec3<T>::null() ); }
00326 inline bool is_null() const { return rot_.is_null() || trn_.is_null(); }
00328 String format() const { return rot_.format() + "\n" + trn_.format(); }
00330
00332
00333 private:
00334 Mat33<T> rot_; Vec3<T> trn_;
00335 };
00336 template<class T> inline Vec3<T> operator *( const RTop<T>& r, const Vec3<T>& v ) { return r.rot()*v + r.trn(); }
00337 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() ); }
00338
00339
00340
00342 template<class T = ftype> class Array2d
00343 {
00344 public:
00346 inline Array2d() { d1_ = d2_ = 0; }
00348 inline Array2d( const int& d1, const int& d2 ) { resize( d1, d2 ); }
00350 inline Array2d( const int& d1, const int& d2, T val )
00351 { resize( d1, d2, val ); }
00353 void inline resize( const int& d1, const int& d2 )
00354 { d1_ = d1; d2_ = d2; data.resize( size() ); }
00356 void inline resize( const int& d1, const int& d2, const T& val )
00357 { d1_ = d1; d2_ = d2; data.resize( size(), val ); }
00358 inline int size() const { return d1_ * d2_; }
00359 inline const int& rows() const { return d1_; }
00360 inline const int& cols() const { return d2_; }
00361
00362 inline const T& operator () ( const int& i1, const int& i2 ) const
00363 { return data[ i1*d2_ + i2 ]; }
00365 inline T& operator () ( const int& i1, const int& i2 )
00366 { return data[ i1*d2_ + i2 ]; }
00367 protected:
00368 std::vector<T> data;
00369 int d1_, d2_;
00370 };
00371
00372
00374 template<class T = ftype> class Matrix : public Array2d<T>
00375 {
00376 public:
00378 inline Matrix() {}
00380 inline Matrix( const int& d1, const int& d2 )
00381 { Array2d<T>::resize( d1, d2 ); }
00383 inline Matrix( const int& d1, const int& d2, T val )
00384 { Array2d<T>::resize( d1, d2, val ); }
00386 std::vector<T> solve( const std::vector<T>& b ) const;
00388 std::vector<T> eigen( const bool sort = true );
00390
00391
00392 };
00393 template<class T> std::vector<T> operator *( const Matrix<T>& m, const std::vector<T>& v )
00394 {
00395 if ( m.cols() != v.size() )
00396 Message::message( Message_fatal( "Matrix*vector dimension mismatch" ) );
00397 std::vector<T> r( m.rows() );
00398 int i, j; T s;
00399 for ( i = 0; i < m.rows(); i++ ) {
00400 s = T(0);
00401 for ( j = 0; j < m.cols(); j++ ) s += m(i,j) * v[j];
00402 r[i] = s;
00403 }
00404 return r;
00405 }
00406
00407
00408
00409
00410
00411 template<class T> bool Vec3<T>::equals( const Vec3<T>& v, const T& tol ) const
00412 {
00413 return ( pow(vec[0]-v[0],T(2)) + pow(vec[1]-v[1],T(2)) +
00414 pow(vec[2]-v[2],T(2)) <= pow(tol,T(2)) );
00415 }
00416
00417 template<class T> template<class TT> Mat33<T>::Mat33( const Mat33<TT>& m )
00418 {
00419 mat[0][0]=T(m(0,0)); mat[0][1]=T(m(0,1)); mat[0][2]=T(m(0,2));
00420 mat[1][0]=T(m(1,0)); mat[1][1]=T(m(1,1)); mat[1][2]=T(m(1,2));
00421 mat[2][0]=T(m(2,0)); mat[2][1]=T(m(2,1)); mat[2][2]=T(m(2,2));
00422 }
00423
00424 template<class T> template<class TT> Mat33<T>::Mat33( const Mat33sym<TT>& m )
00425 {
00426 mat[0][0]=T(m.mat00());
00427 mat[1][1]=T(m.mat11());
00428 mat[2][2]=T(m.mat22());
00429 mat[0][1]=mat[1][0]=T(m.mat01());
00430 mat[0][2]=mat[2][0]=T(m.mat02());
00431 mat[1][2]=mat[2][1]=T(m.mat12());
00432 }
00433
00434 template<class T> bool Mat33<T>::equals( const Mat33<T>& m, const T& tol ) const
00435 {
00436 return ( pow(mat[0][0]-m(0,0),T(2)) + pow(mat[0][1]-m(0,1),T(2)) +
00437 pow(mat[0][2]-m(0,2),T(2)) + pow(mat[1][0]-m(1,0),T(2)) +
00438 pow(mat[1][1]-m(1,1),T(2)) + pow(mat[1][2]-m(1,2),T(2)) +
00439 pow(mat[2][0]-m(2,0),T(2)) + pow(mat[2][1]-m(2,1),T(2)) +
00440 pow(mat[2][2]-m(2,2),T(2)) <= pow(tol,T(2)) );
00441 }
00442
00443 template<class T> T Mat33<T>::det() const
00444 {
00445 return ( mat[0][0]*(mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1]) +
00446 mat[0][1]*(mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2]) +
00447 mat[0][2]*(mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0]) );
00448 }
00449
00450 template<class T> Mat33<T> Mat33<T>::inverse() const
00451 {
00452 T d = det();
00453 Mat33<T> inv;
00454 inv(0,0) = ( mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1] ) / d;
00455 inv(1,0) = ( mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2] ) / d;
00456 inv(2,0) = ( mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0] ) / d;
00457 inv(0,1) = ( mat[2][1]*mat[0][2] - mat[2][2]*mat[0][1] ) / d;
00458 inv(1,1) = ( mat[2][2]*mat[0][0] - mat[2][0]*mat[0][2] ) / d;
00459 inv(2,1) = ( mat[2][0]*mat[0][1] - mat[2][1]*mat[0][0] ) / d;
00460 inv(0,2) = ( mat[0][1]*mat[1][2] - mat[0][2]*mat[1][1] ) / d;
00461 inv(1,2) = ( mat[0][2]*mat[1][0] - mat[0][0]*mat[1][2] ) / d;
00462 inv(2,2) = ( mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0] ) / d;
00463 return inv;
00464 }
00465
00466 template<class T> Mat33<T> Mat33<T>::transpose() const
00467 {
00468 Mat33<T> t;
00469 t(0,0) = mat[0][0]; t(0,1) = mat[1][0]; t(0,2) = mat[2][0];
00470 t(1,0) = mat[0][1]; t(1,1) = mat[1][1]; t(1,2) = mat[2][1];
00471 t(2,0) = mat[0][2]; t(2,1) = mat[1][2]; t(2,2) = mat[2][2];
00472 return t;
00473 }
00474
00475 template<class T> T Mat33sym<T>::det() const
00476 {
00477 return ( m00*(m11*m22 - m12*m12) + m01*(m12*m02 - m01*m22) +
00478 m02*(m01*m12 - m11*m02) );
00479 }
00480
00481 template<class T> Mat33<T> Mat33sym<T>::sqrt() const
00482 {
00483 Mat33<T> half( Mat33sym<T>( 0.5, 0.5, 0.5, 0.0, 0.0, 0.0 ) );
00484 Mat33<T> target( *this );
00485 Mat33<T> result( target );
00486 result(1,0) = result(2,0) = result(2,1) = 0.0;
00487 for ( int i = 0; i < 10; i++ )
00488 result = half * ( result.inverse() * target + result );
00489 return result;
00490 }
00491
00492 template<class T> Mat33sym<T> Mat33sym<T>::inverse() const
00493 {
00494 T d = det();
00495 return Mat33sym<T> ( ( m11*m22 - m12*m12 ) / d,
00496 ( m22*m00 - m02*m02 ) / d,
00497 ( m00*m11 - m01*m01 ) / d,
00498 ( m12*m02 - m22*m01 ) / d,
00499 ( m01*m12 - m02*m11 ) / d,
00500 ( m02*m01 - m00*m12 ) / d );
00501 }
00502
00503 template<class T> T Mat33sym<T>::quad_form( const Vec3<T>& v ) const
00504 {
00505 return ( v[0]*( v[0]*m00 + 2*(v[1]*m01+v[2]*m02) ) +
00506 v[1]*( v[1]*m11 + 2*(v[2]*m12) ) + v[2]*v[2]*m22 );
00507 }
00508
00509 template<class T> const T& Mat33sym<T>::operator ()( const int& i, const int& j ) const
00510 {
00511 switch (i) {
00512 case 0:
00513 switch (j) {
00514 case 0: return m00;
00515 case 1: return m01;
00516 default: return m02;
00517 }
00518 case 1:
00519 switch (j) {
00520 case 0: return m01;
00521 case 1: return m11;
00522 default: return m12;
00523 }
00524 default:
00525 switch (j) {
00526 case 0: return m02;
00527 case 1: return m12;
00528 default: return m22;
00529 }
00530 }
00531 }
00532
00533
00534
00535
00536
00539 template<class T> std::vector<T> Matrix<T>::solve( const std::vector<T>& b ) const
00540 {
00541 if ( Array2d<T>::rows() != Array2d<T>::cols() )
00542 Message::message( Message_fatal("Matrix.solve() matrix not square") );
00543 if ( Array2d<T>::rows() != b.size() )
00544 Message::message( Message_fatal("Matrix.solve() matrix/vector mismatch") );
00545 const int n = Array2d<T>::rows();
00546
00547
00548 T s, pivot;
00549 int i, j, k;
00550
00551 Matrix<T> a = *this;
00552 std::vector<T> x = b;
00553 for ( i = 0; i < n; i++ ) {
00554
00555 j = i;
00556 for ( k = i+1; k < n; k++ )
00557 if ( fabs(a(k,i)) > fabs(a(j,i)) ) j = k;
00558
00559 for ( k = 0; k < n; k++ )
00560 Util::swap( a(i,k), a(j,k) );
00561 Util::swap( x[i], x[j] );
00562
00563 pivot = a(i,i);
00564 for ( j = 0; j < n; j++ ) {
00565 if ( j != i ) {
00566 s = a(j,i) / pivot;
00567 for ( k = i+1; k < n; k++ ) a(j,k) = a(j,k) - s*a(i,k);
00568 x[j] = x[j] - s*x[i];
00569 }
00570 }
00571 }
00572 for ( i = 0; i < n; i++ ) x[i] /= a(i,i);
00573 return x;
00574 }
00575
00576
00582 template<class T> std::vector<T> Matrix<T>::eigen( const bool sort )
00583 {
00584 if ( Array2d<T>::rows() != Array2d<T>::cols() )
00585 Message::message( Message_fatal( "Matrix.eigen() matrix not square" ) );
00586 const int n = Array2d<T>::rows();
00587
00588 int cyc, j, q, p;
00589 T spp, spq, t, s, c, theta, tau, h, ap, aq, a_pq;
00590
00591 Matrix<T>& mat = *this;
00592 Matrix evec( n, n, 0.0 );
00593 std::vector<T> eval( n );
00594 std::vector<T> b( n );
00595 std::vector<T> z( n );
00596
00597
00598 for( p = 0; p < n; p++ ) {
00599 evec(p,p) = 1.0;
00600 eval[p] = b[p] = mat(p,p);
00601 }
00602
00603 for ( cyc = 1; cyc <= 50; cyc++ ) {
00604
00605
00606 spp = spq = 0.0;
00607 for ( p=0; p<n-1; p++ ) {
00608 for ( q=p+1; q<n; q++ )
00609 spq += fabs( mat(p,q) );
00610 spp += fabs( mat(p,p) );
00611 }
00612 if ( spq <= 1.0e-12 * spp ) break;
00613
00614
00615 for ( p = 0; p < n; p++ ) z[p] = 0.0;
00616
00617
00618 for( p=0; p<n-1; p++ ) {
00619 for( q=p+1; q<n; q++ ) {
00620 a_pq = mat( p, q );
00621 h = eval[q] - eval[p];
00622 if ( fabs(a_pq) > 1.0e-12*fabs(h) ) {
00623 theta = 0.5*h/a_pq;
00624 t = 1.0/(fabs(theta) + sqrt(1.0 + theta*theta));
00625 if ( theta < 0.0 ) t = -t;
00626 } else {
00627 t = a_pq/h;
00628 }
00629
00630
00631 c = 1.0/sqrt(1.0+t*t);
00632 s = t*c;
00633 tau = s/(1.0+c);
00634 h = t * a_pq;
00635
00636
00637 z[p] -= h;
00638 z[q] += h;
00639 eval[p] -= h;
00640 eval[q] += h;
00641
00642
00643 mat( p, q ) = 0.0;
00644 for ( j = 0; j < p; j++ ) {
00645 ap = mat( j, p );
00646 aq = mat( j, q );
00647 mat( j, p ) = ap - s * ( aq + ap * tau );
00648 mat( j, q ) = aq + s * ( ap - aq * tau );
00649 }
00650 for ( j = p+1; j < q; j++ ) {
00651 ap = mat( p, j );
00652 aq = mat( j, q );
00653 mat( p, j ) = ap - s * ( aq + ap * tau );
00654 mat( j, q ) = aq + s * ( ap - aq * tau );
00655 }
00656 for ( j = q+1; j < n; j++ ) {
00657 ap = mat( p, j );
00658 aq = mat( q, j );
00659 mat( p, j ) = ap - s * ( aq + ap * tau );
00660 mat( q, j ) = aq + s * ( ap - aq * tau );
00661 }
00662
00663 for ( j = 0; j < n; j++ ) {
00664 ap = evec( j, p );
00665 aq = evec( j, q );
00666 evec( j, p ) = ap - s * ( aq + ap * tau );
00667 evec( j, q ) = aq + s * ( ap - aq * tau );
00668 }
00669 }
00670 }
00671
00672 for ( p = 0; p < n; p++ ) {
00673 b[p] += z[p];
00674 eval[p] = b[p];
00675 }
00676 }
00677
00678
00679 if ( sort ) {
00680 for ( p = 0; p < n; p++ ) {
00681 j = p;
00682 for ( q = p+1; q < n; q++ )
00683 if ( eval[q] < eval[j] ) j = q;
00684 Util::swap( eval[p], eval[j] );
00685 for ( q = 0; q < n; q++ )
00686 Util::swap( evec( q, p ), evec( q, j ) );
00687 }
00688 }
00689
00690
00691 mat = evec;
00692 return eval;
00693 }
00694
00695 }
00696
00697 #endif