00001
00002
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 #include "CeylanMatrix3.h"
00028
00029
00030 #include "CeylanTripoint.h"
00031 #include "CeylanVector3.h"
00032
00033
00034 #include "CeylanLogPlug.h"
00035 #include "CeylanOperators.h"
00036
00037
00038 #ifdef CEYLAN_USES_CONFIG_H
00039 #include "CeylanConfig.h"
00040 #endif // CEYLAN_USES_CONFIG_H
00041
00042
00043 #include <iostream>
00044 using std::endl ;
00045
00046 #include <sstream>
00047 using std::ostringstream ;
00048
00049 #include <iomanip>
00050
00051 using std::string ;
00052
00053
00054 using namespace Ceylan ;
00055 using namespace Ceylan::Maths ;
00056 using namespace Ceylan::Maths::Linear ;
00057
00058 using Ceylan::Maths::Real ;
00059
00060
00061
00062
00063 Matrix3::Matrix3( Real x0, Real x1, Real x2,
00064 Real y0, Real y1, Real y2, Real z0, Real z1, Real z2 )
00065 {
00066
00067 _mat[0][0] = x0 ;
00068 _mat[1][0] = x1 ;
00069 _mat[2][0] = x2 ;
00070 _mat[0][1] = y0 ;
00071 _mat[1][1] = y1 ;
00072 _mat[2][1] = y2 ;
00073 _mat[0][2] = z0 ;
00074 _mat[1][2] = z1 ;
00075 _mat[2][2] = z2 ;
00076
00077 }
00078
00079
00080
00081 Matrix3::Matrix3( const Matrix3 & source ) :
00082 Matrix()
00083 {
00084
00085 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00086 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00087 _mat[i][j] = source._mat[i][j] ;
00088
00089 }
00090
00091
00092
00093 Matrix3::~Matrix3() throw()
00094 {
00095
00096 }
00097
00098
00099
00100 void Matrix3::setTo( Real x0, Real x1, Real x2,
00101 Real y0, Real y1, Real y2, Real z0, Real z1, Real z2 )
00102 {
00103
00104 _mat[0][0] = x0 ;
00105 _mat[1][0] = x1 ;
00106 _mat[2][0] = x2 ;
00107 _mat[0][1] = y0 ;
00108 _mat[1][1] = y1 ;
00109 _mat[2][1] = y2 ;
00110 _mat[0][2] = z0 ;
00111 _mat[1][2] = z1 ;
00112 _mat[2][2] = z2 ;
00113
00114 }
00115
00116
00117
00118 void Matrix3::setColumn( MatrixIndex columnNumber,
00119 const Vector3 & newColumn )
00120 {
00121
00122 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00123 _mat[columnNumber][i] = newColumn._vec[i] ;
00124
00125 }
00126
00127
00128
00129 void Matrix3::setAllElementsTo( Real commonValue )
00130 {
00131
00132 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00133 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00134 _mat[i][j] = commonValue ;
00135
00136 }
00137
00138
00139
00140 Real Matrix3::getElementAt( MatrixIndex abscissa,
00141 MatrixIndex ordinate ) const
00142 {
00143
00144 #if CEYLAN_DEBUG
00145
00146 if ( abscissa >= Dimensions || ordinate >= Dimensions )
00147 throw MathsException( "Matrix3::getElementAt: index out of bounds." ) ;
00148
00149 #endif // CEYLAN_DEBUG
00150
00151 return _mat[ abscissa ][ ordinate ] ;
00152
00153 }
00154
00155
00156
00157 void Matrix3::setElementAt( MatrixIndex abscissa, MatrixIndex ordinate,
00158 Real newValue )
00159 {
00160
00161 #if CEYLAN_DEBUG
00162
00163 if ( abscissa >= Dimensions || ordinate >= Dimensions )
00164 throw MathsException( "Matrix3::setElementAt: index out of bounds." ) ;
00165
00166 #endif // CEYLAN_DEBUG
00167
00168 _mat[ abscissa ][ ordinate ] = newValue ;
00169
00170 }
00171
00172
00173
00174 void Matrix3::setToIdentity()
00175 {
00176
00177 setToDiagonal( 1 ) ;
00178
00179 }
00180
00181
00182
00183 void Matrix3::setToDiagonal( Real diagonalTerm )
00184 {
00185
00186 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00187 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00188 _mat[i][j] = ( (i==j) ? diagonalTerm : 0 ) ;
00189
00190 }
00191
00192
00193
00194 void Matrix3::transpose()
00195 {
00196
00197
00198
00199 Real temp ;
00200
00201 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00202 for ( MatrixIndex i = j+1; i < Dimensions; i++ )
00203 {
00204 temp = _mat[j][i] ;
00205 _mat[j][i] = _mat[i][j] ;
00206 _mat[i][j] = temp ;
00207 }
00208
00209 }
00210
00211
00212
00213 Real Matrix3::trace() const
00214 {
00215
00216 Real sum = 0 ;
00217
00218 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00219 sum += _mat[i][i] ;
00220
00221 return sum ;
00222
00223 }
00224
00225
00226
00227 Real Matrix3::determinant() const
00228 {
00229
00230 return _mat[0][0] * ( _mat[1][1] * _mat[2][2] - _mat[1][2] * _mat[2][1] )
00231 - _mat[0][1] * ( _mat[1][0] * _mat[2][2] - _mat[1][2] * _mat[2][0] )
00232 + _mat[0][2] * ( _mat[1][0] * _mat[2][1] - _mat[1][1] * _mat[2][0] ) ;
00233
00234 }
00235
00236
00237
00238 const string Matrix3::toString( VerbosityLevels level ) const
00239 {
00240
00241 string res ;
00242
00243 if ( TextDisplayable::GetOutputFormat() == TextDisplayable::html )
00244 {
00245
00246 res = "<table border=1>" ;
00247
00248 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00249 {
00250 res += " <tr>\n" ;
00251
00252 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00253 res += " <td>" + Ceylan::toString( _mat[i][j] )
00254 + "</td>" ;
00255
00256 res += " </tr>\n" ;
00257 }
00258
00259 res += "</table>" ;
00260
00261
00262 return res ;
00263 }
00264
00265
00266
00267 ostringstream oss ;
00268
00269 oss.precision( Ceylan::DigitOutputPrecision ) ;
00270
00271 oss << endl ;
00272
00273 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00274 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00275 oss << ( ( i == 0 ) ? "[ " : " " )
00276 << std::setw(5)
00277 << _mat[i][j]
00278 << ( ( i== Dimensions-1 ) ? " ]\n" : " ") ;
00279 oss << endl ;
00280
00281 res = oss.str() ;
00282
00283
00284 #if CEYLAN_DEBUG
00285
00286 if ( oss.fail() )
00287 {
00288 string message = "Matrix3::toString: conversion error." ;
00289 Log::LogPlug::error( message ) ;
00290 return message ;
00291 }
00292
00293 #endif // CEYLAN_DEBUG
00294
00295 return res ;
00296
00297 }
00298
00299
00300
00301 Matrix3 Matrix3::Cofactor( const Matrix3 & m )
00302 {
00303
00304 Matrix3 result ;
00305
00306 result._mat[0][0] =
00307 m._mat[1][1] * m._mat[2][2] - m._mat[1][2] * m._mat[2][1] ;
00308
00309 result._mat[1][0] =
00310 m._mat[0][2] * m._mat[2][1] - m._mat[0][1] * m._mat[2][2] ;
00311
00312 result._mat[2][0] =
00313 m._mat[0][1] * m._mat[1][2] - m._mat[0][2] * m._mat[1][1] ;
00314
00315 result._mat[0][1] =
00316 m._mat[1][2] * m._mat[2][0] - m._mat[1][0] * m._mat[2][2] ;
00317
00318 result._mat[1][1] =
00319 m._mat[0][0] * m._mat[2][2] - m._mat[0][2] * m._mat[2][0] ;
00320
00321 result._mat[2][1] =
00322 m._mat[0][2] * m._mat[1][0] - m._mat[0][0] * m._mat[1][2] ;
00323
00324 result._mat[0][2] =
00325 m._mat[1][0] * m._mat[2][1] - m._mat[1][1] * m._mat[2][0] ;
00326
00327 result._mat[1][2] =
00328 m._mat[0][1] * m._mat[2][0] - m._mat[0][0] * m._mat[2][1] ;
00329
00330 result._mat[2][2] =
00331 m._mat[0][0] * m._mat[1][1] - m._mat[0][1] * m._mat[1][0] ;
00332
00333 return result ;
00334
00335 }
00336
00337
00338
00339 Matrix3 Matrix3::Adjoint( const Matrix3 & m )
00340 {
00341
00342 Matrix3 result = Cofactor( m ) ;
00343 result.transpose() ;
00344 return result ;
00345
00346
00347
00348 }
00349
00350
00351
00352 bool Ceylan::Maths::Linear::operator == ( const Matrix3 & m1,
00353 const Matrix3 & m2 )
00354 {
00355
00356 for ( MatrixIndex j = 0; j < Matrix3::Dimensions; j++ )
00357 for ( MatrixIndex i = 0; i < Matrix3::Dimensions; i++ )
00358 {
00359 if ( ! AreRelativelyEqual<Real>( m1._mat[i][j],
00360 m2._mat[i][j] ) )
00361 return false ;
00362 }
00363
00364 return true ;
00365
00366 }
00367
00368
00369
00370 bool Ceylan::Maths::Linear::operator != ( const Matrix3 & m1,
00371 const Matrix3 & m2 )
00372 {
00373
00374 return ( ! ( m1 == m2 ) ) ;
00375
00376 }
00377
00378
00379
00380 Matrix3 Ceylan::Maths::Linear::operator + ( const Matrix3 & m1,
00381 const Matrix3 & m2 )
00382 {
00383
00384 Matrix3 result ;
00385
00386 for ( MatrixIndex j = 0; j < Matrix3::Dimensions; j++ )
00387 for ( MatrixIndex i = 0; i < Matrix3::Dimensions; i++ )
00388 result._mat[i][j] = m1._mat[i][j] + m2._mat[i][j] ;
00389
00390 return result ;
00391
00392 }
00393
00394
00395
00396 Matrix3 Ceylan::Maths::Linear::operator - ( const Matrix3 & m1,
00397 const Matrix3 & m2 )
00398 {
00399
00400 Matrix3 result ;
00401
00402 for ( MatrixIndex j = 0; j < Matrix3::Dimensions; j++ )
00403 for ( MatrixIndex i = 0; i < Matrix3::Dimensions; i++ )
00404 result._mat[i][j] = m1._mat[i][j] - m2._mat[i][j] ;
00405
00406 return result ;
00407
00408 }
00409
00410
00411
00412 Matrix3 Ceylan::Maths::Linear::operator * ( const Matrix3 & m1,
00413 const Matrix3 & m2 )
00414 {
00415
00416 Matrix3 result ;
00417
00418 for ( MatrixIndex j = 0; j < Matrix3::Dimensions; j++ )
00419 for ( MatrixIndex i = 0; i < Matrix3::Dimensions; i++ )
00420 for ( MatrixIndex k = 0; k < Matrix3::Dimensions; k++ )
00421 result._mat[i][j] += m1._mat[k][j] * m2._mat[i][k] ;
00422
00423 return result ;
00424
00425 }
00426
00427
00428
00429 Matrix3 Ceylan::Maths::Linear::operator * ( Real lambda, const Matrix3 & m )
00430 {
00431
00432 Matrix3 result ;
00433
00434 for ( MatrixIndex j = 0; j < Matrix3::Dimensions; j++ )
00435 for ( MatrixIndex i = 0; i < Matrix3::Dimensions; i++ )
00436 result._mat[i][j] = lambda * m._mat[i][j] ;
00437
00438 return result ;
00439
00440 }
00441
00442
00443
00444 Matrix3 Ceylan::Maths::Linear::operator ! ( const Matrix3 & m )
00445 {
00446
00447
00448
00449
00450
00451 Real d = m.determinant() ;
00452
00453 if ( ! IsNull( d ) )
00454 return (1/d) * ~ Matrix3::Cofactor( m ) ;
00455 else
00456 throw LinearException(
00457 "Matrix3: inverse operator ('!'): singular matrix." ) ;
00458
00459 }
00460
00461
00462
00463 Matrix3 Ceylan::Maths::Linear::operator ~ ( const Matrix3 & m )
00464 {
00465
00466
00467
00468 Matrix3 result( m ) ;
00469
00470 result.transpose() ;
00471
00472 return result ;
00473
00474 }
00475