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 "CeylanHomogeneousMatrix3.h"
00028
00029 #include "CeylanMatrix2.h"
00030 #include "CeylanVector2.h"
00031
00032 #include "CeylanTripoint.h"
00033 #include "CeylanVector3.h"
00034
00035 #include "CeylanLogPlug.h"
00036 #include "CeylanOperators.h"
00037 #include "CeylanTextDisplayable.h"
00038
00039 #ifdef CEYLAN_USES_CONFIG_H
00040 #include "CeylanConfig.h"
00041 #endif // CEYLAN_USES_CONFIG_H
00042
00043 #include <iostream>
00044 using std::endl ;
00045
00046 #include <sstream>
00047 using std::ostringstream ;
00048
00049 #include <iomanip>
00050
00051
00052 using std::string ;
00053
00054 using namespace Ceylan ;
00055 using namespace Ceylan::Maths ;
00056 using namespace Ceylan::Maths::Linear ;
00057
00058
00059
00060
00061 HomogeneousMatrix3::HomogeneousMatrix3( const Matrix2 & r,
00062 const Vector2 & v )
00063 {
00064
00065
00066 setRotationMatrix( r ) ;
00067
00068
00069 setTranslationVector( v ) ;
00070
00071 setBottomRow() ;
00072
00073 }
00074
00075
00076
00077 HomogeneousMatrix3::HomogeneousMatrix3( Real r0, Real r1, Real r2, Real r3,
00078 Real t0, Real t1 )
00079 {
00080
00081
00082 _mat[0][0] = r0 ;
00083 _mat[1][0] = r1 ;
00084 _mat[0][1] = r2 ;
00085 _mat[1][1] = r3 ;
00086
00087
00088 _mat[2][0] = t0 ;
00089 _mat[2][1] = t1 ;
00090
00091 setBottomRow() ;
00092
00093 }
00094
00095
00096
00097 HomogeneousMatrix3::HomogeneousMatrix3( AngleInDegrees angle,
00098 const Maths::Linear::Vector2 & v )
00099 {
00100
00101 setRotationMatrix( Matrix2::CreateFromRotation( angle ) ) ;
00102 setTranslationVector( v ) ;
00103 setBottomRow() ;
00104
00105 }
00106
00107
00108
00109 HomogeneousMatrix3::~HomogeneousMatrix3() throw()
00110 {
00111
00112
00113
00114 }
00115
00116
00117
00118 const string HomogeneousMatrix3::toString( VerbosityLevels level ) const
00119 {
00120
00121
00122 string res ;
00123
00124 if ( TextDisplayable::GetOutputFormat() == TextDisplayable::html )
00125 {
00126
00127 res = "<table border=1>" ;
00128
00129 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00130 {
00131 res += " <tr>\n" ;
00132
00133 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00134 res += " <td>" + Ceylan::toString( _mat[i][j] ) + "</td>" ;
00135
00136 res += " </tr>\n" ;
00137 }
00138
00139 res += "</table>" ;
00140
00141
00142 return res ;
00143
00144 }
00145
00146
00147
00148 ostringstream oss ;
00149
00150 oss.precision( Ceylan::DigitOutputPrecision ) ;
00151
00152 oss << endl ;
00153
00154 for ( MatrixIndex j = 0; j < Dimensions; j++ )
00155 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00156 oss << ( ( i == 0 ) ? "[ " : " " )
00157 << std::setw(5)
00158 << _mat[i][j]
00159 << ( ( i == Dimensions-1 ) ? " ]\n" : " ") ;
00160 oss << endl ;
00161
00162 res = oss.str() ;
00163
00164
00165 #if CEYLAN_DEBUG
00166
00167 if ( oss.fail() )
00168 {
00169 string message = "HomogeneousMatrix3::toString: conversion error." ;
00170 Log::LogPlug::error( message ) ;
00171 return message ;
00172 }
00173
00174 #endif // CEYLAN_DEBUG
00175
00176 return res ;
00177
00178 }
00179
00180
00181
00182 void HomogeneousMatrix3::setBottomRow()
00183 {
00184
00185
00186 _mat[0][2] = 0 ;
00187 _mat[1][2] = 0 ;
00188
00189
00190 _mat[2][2] = 1 ;
00191
00192 }
00193
00194
00195 void HomogeneousMatrix3::setTranslationVector( const Vector2 & v )
00196 {
00197
00198 _mat[2][0] = v.getElementAt( 0 ) ;
00199 _mat[2][1] = v.getElementAt( 1 ) ;
00200
00201 }
00202
00203
00204
00205 void HomogeneousMatrix3::setRotationMatrix( const Matrix2 & m )
00206 {
00207
00208 _mat[0][0] = m.getElementAt( 0, 0 ) ;
00209 _mat[1][0] = m.getElementAt( 1, 0 ) ;
00210 _mat[0][1] = m.getElementAt( 0, 1 ) ;
00211 _mat[1][1] = m.getElementAt( 1, 1 ) ;
00212
00213 }
00214
00215
00216
00217 void HomogeneousMatrix3::setInCanonicalForm()
00218 {
00219
00220
00221
00222 Real bottomRightElement = _mat[Dimensions-1][Dimensions-1] ;
00223
00224 if ( IsNull( bottomRightElement ) )
00225 throw LinearException( "HomogeneousMatrix3::setInCanonicalForm: "
00226 "bottom-right element is zero or almost." ) ;
00227
00228 if ( AreRelativelyEqual<Real>( bottomRightElement, 1 ) )
00229 {
00230
00231
00232 return ;
00233
00234 }
00235
00236
00237
00238
00239
00240
00241
00242
00243 for ( MatrixIndex j = 0; j < Dimensions - 1; j++ )
00244 for ( MatrixIndex i = 0; i < Dimensions; i++ )
00245 _mat[i][j] /= bottomRightElement ;
00246
00247
00248
00249 _mat[Dimensions-1][Dimensions-1] = 1 ;
00250
00251 }
00252
00253
00254
00255 HomogeneousMatrix3 Ceylan::Maths::Linear::operator * (
00256 const HomogeneousMatrix3 & m1,
00257 const HomogeneousMatrix3 & m2 )
00258 {
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270 HomogeneousMatrix3 result ;
00271
00272
00273 for ( MatrixIndex j = 0; j < Matrix2::Dimensions; j++ )
00274 for ( MatrixIndex i = 0; i < Matrix2::Dimensions; i++ )
00275 for ( MatrixIndex k = 0; k < Matrix2::Dimensions; k++ )
00276 result._mat[i][j] += m1._mat[k][j] * m2._mat[i][k] ;
00277
00278
00279
00280 for ( MatrixIndex j = 0 ; j < Matrix2::Dimensions ; j++ )
00281 {
00282 result._mat[2][j] = m1._mat[0][j] * m2._mat[2][0]
00283 + m1._mat[1][j] * m2._mat[2][1]
00284 + m1._mat[2][j] ;
00285 }
00286
00287
00288 result.setBottomRow() ;
00289
00290 return result ;
00291
00292 }
00293