00001
00002
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00031
00032 #ifndef RCSC_GEOM_MATRIX2D_H
00033 #define RCSC_GEOM_MATRIX2D_H
00034
00035 #include <rcsc/geom/vector_2d.h>
00036 #include <rcsc/geom/angle_deg.h>
00037
00038 #include <iostream>
00039 #include <cmath>
00040
00041 namespace rcsc {
00042
00051 class Matrix2D {
00052 private:
00053
00054 double M_11;
00055 double M_12;
00056 double M_21;
00057 double M_22;
00058 double M_dx;
00059 double M_dy;
00060
00061 public:
00062
00066 Matrix2D()
00067 : M_11( 1.0 )
00068 , M_12( 0.0 )
00069 , M_21( 0.0 )
00070 , M_22( 1.0 )
00071 , M_dx( 0.0 )
00072 , M_dy( 0.0 )
00073 { }
00074
00084 Matrix2D( const double & m11, const double & m12,
00085 const double & m21, const double & m22,
00086 const double & dx, const double & dy )
00087 : M_11( m11 ), M_12( m12 )
00088 , M_21( m21 ), M_22( m22 )
00089 , M_dx( dx ), M_dy( dy )
00090 { }
00091
00096 const
00097 Matrix2D & reset()
00098 {
00099 M_11 = M_22 = 1.0;
00100 M_12 = M_21 = M_dx = M_dy = 0.0;
00101 return *this;
00102 }
00103
00114 const
00115 Matrix2D & assign( const double & m11, const double & m12,
00116 const double & m21, const double & m22,
00117 const double & dx, const double & dy )
00118 {
00119 M_11 = m11; M_12 = m12;
00120 M_21 = m21; M_22 = m22;
00121 M_dx = dx; M_dy = dy;
00122 return *this;
00123 }
00124
00131 static
00132 Matrix2D make_translation( const double & dx,
00133 const double & dy )
00134 {
00135 return Matrix2D( 1.0, 0.0,
00136 0.0, 1.0,
00137 dx, dy );
00138 }
00139
00146 static
00147 Matrix2D make_scaling( const double & sx,
00148 const double & sy )
00149 {
00150 return Matrix2D( sx, 0.0,
00151 0.0, sy,
00152 0.0, 0.0 );
00153 }
00154
00160 static
00161 Matrix2D make_rotation( const AngleDeg & angle )
00162 {
00163 double cosa = angle.cos();
00164 double sina = angle.sin();
00165 return Matrix2D( cosa, -sina,
00166 sina, cosa,
00167 0.0, 0.0 );
00168 }
00169
00174 const
00175 double & m11() const
00176 {
00177 return M_11;
00178 }
00179
00184 const
00185 double & m12() const
00186 {
00187 return M_12;
00188 }
00189
00194 const
00195 double & m21() const
00196 {
00197 return M_21;
00198 }
00199
00204 const
00205 double & m22() const
00206 {
00207 return M_22;
00208 }
00209
00214 const
00215 double & dx() const
00216 {
00217 return M_dx;
00218 }
00219
00224 const
00225 double & dy() const
00226 {
00227 return M_dy;
00228 }
00229
00234 double det() const
00235 {
00236 return M_11*M_22 - M_12*M_21;
00237 }
00238
00243 bool invertible() const
00244 {
00245 return ! ( std::fabs( det() ) < 0.00000000001 );
00246 }
00247
00252 Matrix2D inverted() const;
00253
00263 const
00264 Matrix2D & translate( const double & dx,
00265 const double & dy )
00266 {
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281 M_dx += dx;
00282 M_dy += dy;
00283 return *this;
00284 }
00285
00295 const
00296 Matrix2D & scale( const double & sx,
00297 const double & sy )
00298 {
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314 M_11 *= sx; M_12 *= sx; M_dx *= sx;
00315 M_21 *= sy; M_22 *= sy; M_dy *= sy;
00316 return *this;
00317 }
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00342 const
00343 Matrix2D & rotate( const AngleDeg & angle );
00344
00350 const
00351 Matrix2D & operator*=( const Matrix2D & m )
00352 {
00353 double tm11 = M_11*m.M_11 + M_12*m.M_21;
00354 double tm12 = M_11*m.M_12 + M_12*m.M_22;
00355 double tm21 = M_21*m.M_11 + M_22*m.M_21;
00356 double tm22 = M_21*m.M_12 + M_22*m.M_22;
00357
00358 double tdx = M_11*m.M_dx + M_12*m.M_dy + M_dx;
00359 double tdy = M_21*m.M_dx + M_22*m.M_dy + M_dy;
00360
00361 M_11 = tm11; M_12 = tm12;
00362 M_21 = tm21; M_22 = tm22;
00363 M_dx = tdx; M_dy = tdy;
00364 return *this;
00365 }
00366
00372 Vector2D transform( const Vector2D & v ) const
00373 {
00374 return Vector2D( M_11*v.x + M_12*v.y + M_dx,
00375 M_21*v.x + M_22*v.y + M_dy );
00376 }
00377
00384 Vector2D transform( const double & x,
00385 const double & y ) const
00386 {
00387 return Vector2D( M_11*x + M_12*y + M_dx,
00388 M_21*x + M_22*y + M_dy );
00389 }
00390
00395 void transform( Vector2D * v ) const
00396 {
00397 double tx = M_11*v->x + M_12*v->y + M_dx;
00398 double ty = M_21*v->x + M_22*v->y + M_dy;
00399 v->assign( tx, ty );
00400 }
00401
00402 #if 0
00403 Segment2D transform( const Segment2D & s ) const
00404 {
00405 return Segment2D( transform( s.a() ),
00406 transform( s.b() ) );
00407 }
00408
00409
00410
00411
00412
00413
00414
00415 Rai2D transform( const Ray2D & r ) const
00416 {
00417 return Ray2D( transform( r.origin() ),
00418 transform( r.origin() + Vector2D::polar2vector( 1.0, r.dir() ) ) );
00419 }
00420
00421 Circle2D transform( const Circle2D & c ) const
00422 {
00423 return Circle2D( transform( c.center() ),
00424 c.radius() );
00425 }
00426
00427
00428
00429
00430
00431
00432
00433
00434 Triangle2D transform( const Triangle2D & t ) const
00435 {
00436 return Triangle2D( transform( t.a() ),
00437 transform( t.b() ),
00438 transform( t.c() ) );
00439 }
00440 #endif
00441
00447 std::ostream & print( std::ostream & os ) const
00448 {
00449 os << M_11 << ' '
00450 << M_12 << ' '
00451 << M_21 << ' '
00452 << M_22 << ' '
00453 << M_dx << ' '
00454 << M_dy;
00455 return os;
00456 }
00457
00458 };
00459
00460 }
00461
00468 inline
00469 const
00470 rcsc::Matrix2D
00471 operator*( const rcsc::Matrix2D & lhs,
00472 const rcsc::Matrix2D & rhs )
00473 {
00474 return rcsc::Matrix2D( lhs ) *= rhs;
00475 }
00476
00483 inline
00484 rcsc::Vector2D
00485 operator*( const rcsc::Matrix2D & lhs,
00486 const rcsc::Vector2D & rhs )
00487 {
00488 return lhs.transform( rhs );
00489 }
00490
00497 inline
00498 std::ostream &
00499 operator<<( std::ostream & os,
00500 const rcsc::Matrix2D & m )
00501 {
00502 return m.print( os );
00503 }
00504
00505
00506 #endif