Matrix3.cpp

Go to the documentation of this file.
00001 // -*- c-basic-offset: 4 -*-
00024 #include "Matrix3.h"
00025 
00026 inline Matrix3 getIdentity()
00027 {
00028     Matrix3 tmp;
00029     tmp.SetIdentity();
00030     return tmp;
00031 }
00032 Matrix3 Matrix3::Identity = getIdentity();
00033 
00034 
00036 Matrix3::Matrix3() {
00037     for (int i=0; i<3; i++)
00038         for (int j=0; j<3; j++)
00039             m[i][j] = 0.0;
00040 }
00041 
00043 Matrix3::Matrix3(const Matrix3& ot)
00044 {
00045     (*this) = ot; // call copy operator
00046 }
00047 
00049 void Matrix3::SetIdentity()
00050 {
00051     m[0][0] = 1; m[0][1] = 0;  m[0][2] = 0;
00052     m[1][0] = 0; m[1][1] = 1;  m[1][2] = 0;
00053     m[2][0] = 0; m[2][1] = 0;  m[2][2] = 1;
00054 }
00055 
00057 void Matrix3::SetRotation( double Yaw, double Pitch, double Roll )
00058 {
00059     double      SR      = sin(Roll),
00060             SP  = sin(Pitch),
00061             SY  = sin(Yaw),
00062             CR  = cos(Roll),
00063             CP  = cos(Pitch),
00064             CY  = cos(Yaw);
00065 
00066     m[0][0]     = CP * CY;
00067     m[0][1]     = CP * SY;
00068     m[0][2]     = SP;
00069     
00070     m[1][0]     = SR * SP * CY - CR * SY;
00071     m[1][1]     = SR * SP * SY + CR * CY;
00072     m[1][2]     = - SR * CP;
00073     
00074     m[2][0]     = -( CR * SP * CY + SR * SY );
00075     m[2][1]     = CY * SR - CR * SP * SY;
00076     m[2][2]     = CR * CP;
00077 }
00078 
00079 #if 0
00080 
00083 Matrix3::void SetRotationPT( double Yaw, double Pitch, double Roll )
00084 {
00085     double  SR  = sin(Roll),
00086     SP  = sin(Pitch),
00087     SY  = sin(Yaw),
00088     CR  = cos(Roll),
00089     CP  = cos(Pitch),
00090     CY  = cos(Yaw);
00091 
00092     m[0][0] = CP * CY;
00093     m[0][1] = CP * SY;
00094     m[0][2] = -SP;
00095 
00096     m[1][0] = SR * SP * CY - CR * SY;
00097     m[1][1] = SR * SP * SY + CR * CY;
00098     m[1][2] = SR * CP;
00099 
00100     m[2][0] = CR * SP * CY + SR * SY;
00101     m[2][1] = CR * SP * SY - CY * SR;
00102     m[2][2] = CR * CP;
00103 }
00104 #endif 
00105 
00106 
00110 void Matrix3::SetRotationPT( double yaw, double pitch, double roll )
00111 {
00112     double cosr = cos (roll);
00113     double sinr = sin (roll);
00114     double cosp = cos (pitch);
00115     double sinp = sin (0 - pitch);
00116     double cosy = cos (yaw);
00117     double siny = sin (0 - yaw);
00118 
00119     Matrix3 rollm;
00120 
00121     /*
00122     rollm[0][0] = new Math::Matrix ([        1,       0,       0 ],
00123             [        0,   cosr,-1*sinr ],
00124             [        0,   sinr,   cosr ]);
00125     */
00126 
00127     rollm.m[0][0] = 1.0;      rollm.m[0][1] = 0.0;      rollm.m[0][2] = 0.0;
00128     rollm.m[1][0] = 0.0;      rollm.m[1][1] = cosr;     rollm.m[1][2] = -sinr;
00129     rollm.m[2][0] = 0.0;      rollm.m[2][1] = sinr;     rollm.m[2][2] = cosr;
00130 
00131     /*
00132 my pitchm = new Math::Matrix ([    cosp,       0,   sinp ],
00133                                     [        0,       1,       0 ],
00134                                     [ -1*sinp,       0,   cosp ]);
00135     */
00136     
00137     Matrix3 pitchm;
00138     pitchm.m[0][0] = cosp;   pitchm.m[0][1] =  0.0;  pitchm.m[0][2] = sinp;
00139     pitchm.m[1][0] =  0.0;   pitchm.m[1][1] =    1;  pitchm.m[1][2] = 0.0;
00140     pitchm.m[2][0] = -sinp;  pitchm.m[2][1] =  0.0;  pitchm.m[2][2] = cosp;
00141 
00142     /*
00143 my yawm   = new Math::Matrix ([    cosy,-1*siny,       0 ],
00144                                     [    siny,   cosy,       0 ],
00145                                     [        0,       0,       1 ]);
00146     */
00147     Matrix3 yawm;
00148     yawm.m[0][0] = cosy;   yawm.m[0][1] = -siny;   yawm.m[0][2] = 0.0;
00149     yawm.m[1][0] = siny;   yawm.m[1][1] =  cosy;   yawm.m[1][2] = 0.0;
00150     yawm.m[2][0] = 0.0;    yawm.m[2][1] =   0.0;   yawm.m[2][2] = 1.0;
00151 
00152 
00153     *this = yawm * pitchm * rollm;
00154 }
00155 
00157 void Matrix3::GetRotationPT( double & Yaw, double & Pitch, double & Roll )
00158 {
00159     /*
00160     my $matrix = shift;
00161     my $roll = atan2 ($matrix->[2]->[1], $matrix->[2]->[2]);
00162     my $pitch = -1 * asin (-1 * $matrix->[2]->[0]);
00163     my $yaw = atan2 (-1 * $matrix->[1]->[0], $matrix->[0]->[0]);
00164     return ($roll, $pitch, $yaw);
00165 
00166     */
00167     Roll = atan2 (m[2][1], m[2][2]);
00168     Pitch = - asin (- m[2][0]);
00169     Yaw = atan2 (- m[1][0], m[0][0]);
00170 }
00171 
00173 void Matrix3::SetRotationX( double a )
00174 {
00175     m[0][0] = 1.0;              m[0][1] = 0.0;          m[0][2] = 0.0;
00176     m[1][0] = 0.0;              m[1][1] = cos(a);       m[1][2] = sin(a);
00177     m[2][0] = 0.0;              m[2][1] =-m[1][2];      m[2][2] = m[1][1];
00178 }
00179 
00180 void Matrix3::SetRotationY( double a )
00181 {
00182     m[0][0] = cos(a);   m[0][1] = 0.0;          m[0][2] =-sin(a);
00183     m[1][0] = 0.0;              m[1][1] = 1.0;          m[1][2] = 0.0;
00184     m[2][0] =-m[0][2];  m[2][1] = 0.0;          m[2][2] = m[0][0];
00185 }
00186 
00187 void Matrix3::SetRotationZ( double a )
00188 {
00189     m[0][0] = cos(a);   m[0][1] = sin(a);       m[0][2] = 0.0;
00190     m[1][0] =-m[0][1];  m[1][1] = m[0][0];      m[1][2] = 0.0;
00191     m[2][0] = 0.0;              m[2][1] = 0.0;          m[2][2] = 1.0;
00192 }
00193 
00195 Matrix3& Matrix3::operator= (const Matrix3& ot)
00196 {
00197     for (int i=0; i<3; i++)
00198         for (int j=0; j<3; j++)
00199             m[i][j] = ot.m[i][j];
00200     return *this;
00201 }
00202 
00204 Matrix3 Matrix3::operator*(const Matrix3& ot) const
00205 {
00206     Matrix3     Result;
00207     Result.m[0][0] = m[0][0] * ot.m[0][0] + m[0][1] * ot.m[1][0] + m[0][2] * ot.m[2][0];
00208     Result.m[0][1] = m[0][0] * ot.m[0][1] + m[0][1] * ot.m[1][1] + m[0][2] * ot.m[2][1];
00209     Result.m[0][2] = m[0][0] * ot.m[0][2] + m[0][1] * ot.m[1][2] + m[0][2] * ot.m[2][2];
00210     
00211     Result.m[1][0] = m[1][0] * ot.m[0][0] + m[1][1] * ot.m[1][0] + m[1][2] * ot.m[2][0];
00212     Result.m[1][1] = m[1][0] * ot.m[0][1] + m[1][1] * ot.m[1][1] + m[1][2] * ot.m[2][1];
00213     Result.m[1][2] = m[1][0] * ot.m[0][2] + m[1][1] * ot.m[1][2] + m[1][2] * ot.m[2][2];
00214     
00215     Result.m[2][0] = m[2][0] * ot.m[0][0] + m[2][1] * ot.m[1][0] + m[2][2] * ot.m[2][0];
00216     Result.m[2][1] = m[2][0] * ot.m[0][1] + m[2][1] * ot.m[1][1] + m[2][2] * ot.m[2][1];
00217     Result.m[2][2] = m[2][0] * ot.m[0][2] + m[2][1] * ot.m[1][2] + m[2][2] * ot.m[2][2];
00218     return Result;
00219 }
00220 
00222 void Matrix3::operator/=(double s)
00223 {
00224     for (int i=0; i<3; i++)
00225         for (int j=0; j<3; j++)
00226             m[i][j] /= s;
00227 }
00228 
00230 void Matrix3::operator*=(double s)
00231 {
00232     for (int i=0; i<3; i++)
00233         for (int j=0; j<3; j++)
00234             m[i][j] *= s;
00235 }
00236 
00238 void Matrix3::operator*=(Matrix3 ot)
00239 {
00240     Matrix3 Result;
00241     Result.m[0][0] = m[0][0] * ot.m[0][0] + m[0][1] * ot.m[1][0] + m[0][2] * ot.m[2][0];
00242     Result.m[0][1] = m[0][0] * ot.m[0][1] + m[0][1] * ot.m[1][1] + m[0][2] * ot.m[2][1];
00243     Result.m[0][2] = m[0][0] * ot.m[0][2] + m[0][1] * ot.m[1][2] + m[0][2] * ot.m[2][2];
00244     
00245     Result.m[1][0] = m[1][0] * ot.m[0][0] + m[1][1] * ot.m[1][0] + m[1][2] * ot.m[2][0];
00246     Result.m[1][1] = m[1][0] * ot.m[0][1] + m[1][1] * ot.m[1][1] + m[1][2] * ot.m[2][1];
00247     Result.m[1][2] = m[1][0] * ot.m[0][2] + m[1][1] * ot.m[1][2] + m[1][2] * ot.m[2][2];
00248     
00249     Result.m[2][0] = m[2][0] * ot.m[0][0] + m[2][1] * ot.m[1][0] + m[2][2] * ot.m[2][0];
00250     Result.m[2][1] = m[2][0] * ot.m[0][1] + m[2][1] * ot.m[1][1] + m[2][2] * ot.m[2][1];
00251     Result.m[2][2] = m[2][0] * ot.m[0][2] + m[2][1] * ot.m[1][2] + m[2][2] * ot.m[2][2];
00252     *this = Result;
00253 }
00254 
00256 Matrix3 Matrix3::Inverse() const
00257 {
00258     Matrix3 Result;
00259     double Det = Determinant();
00260 
00261     if (Det == 0.0f)
00262         return Matrix3::Identity;
00263 
00264     double      invDet = 1.0f / Det;
00265 
00266     Result.m[0][0] =  invDet * ( m[1][1] * m[2][2] - m[2][1] * m[1][2] );
00267     Result.m[0][1] = -invDet * ( m[0][1] * m[2][2] - m[2][1] * m[0][2] );
00268     Result.m[0][2] =  invDet * ( m[0][1] * m[1][2] - m[1][1] * m[0][2] );
00269     
00270     Result.m[1][0] = -invDet * ( m[1][0] * m[2][2] - m[2][0] * m[1][2] );
00271     Result.m[1][1] =  invDet * ( m[0][0] * m[2][2] - m[2][0] * m[0][2] );
00272     Result.m[1][2] = -invDet * ( m[0][0] * m[1][2] - m[1][0] * m[0][2] );
00273     
00274     Result.m[2][0] =  invDet * ( m[1][0] * m[2][1] - m[2][0] * m[1][1] );
00275     Result.m[2][1] = -invDet * ( m[0][0] * m[2][1] - m[2][0] * m[0][1] );
00276     Result.m[2][2] =  invDet * ( m[0][0] * m[1][1] - m[1][0] * m[0][1] );
00277     return Result;
00278 }
00279 
00280 void Matrix3::Print(std::ostream & o) const
00281 {
00282     o << "[ " << m[0][0] << "\t" << m[0][1] << "\t" <<  m[0][2] << std::endl
00283       << "  " << m[1][0] << "\t" << m[1][1] << "\t" <<  m[1][2] << std::endl
00284       << "  " << m[2][0] << "\t" << m[2][1] << "\t" <<  m[2][2] << std::endl;
00285 }
00286 
00287 
00288 /*// return the rotation matrix around X
00289 Matrix3 GetRotationX(double Ang)
00290 {
00291         double a = cos(Ang);
00292         double b = sin(Ang);
00293         return Matrix3(
00294                         1.0,    0.0,            0.0,
00295                         0.0,    a,                      b,
00296                         0.0,    -b,                     a );
00297 }
00298 
00299 //return the rotation matrix around Y
00300 Matrix3 GetRotationY(double Ang)
00301 {
00302         double a = cos(Ang);
00303         double b = -sin(Ang);
00304         return Matrix3(
00305                         a,              0.0,    b,
00306                         0.0,    1.0,    0.0,
00307                         -b,             0.0,    a );
00308 }
00309 
00310 //return the rotation matrix around Z
00311 Matrix3 GetRotationZ(double Ang)
00312 {
00313         double a = cos(Ang);
00314         double b = sin(Ang);
00315         return Matrix3(
00316                         a,              b,              0.0,
00317                         -b,             a,              0.0,
00318                         0.0,    0.0,    1.0);
00319 }
00320 */

Generated on Wed Apr 16 01:25:37 2014 for Hugintrunk by  doxygen 1.3.9.1