IceMatrix4x4.h
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 // Include Guard
00012 #ifndef __ICEMATRIX4X4_H__
00013 #define __ICEMATRIX4X4_H__
00014 
00015         // Forward declarations
00016         class PRS;
00017         class PR;
00018 
00019         #define MATRIX4X4_EPSILON               (1.0e-7f)
00020 
00021         class ICEMATHS_API Matrix4x4
00022         {
00023 //                              void    LUBackwardSubstitution( sdword *indx, float* b );
00024 //                              void    LUDecomposition( sdword* indx, float* d );
00025 
00026                 public:
00028                 inline_                                         Matrix4x4()                                                                     {}
00030                 inline_                                         Matrix4x4(      float m00, float m01, float m02, float m03,
00031                                                                                                 float m10, float m11, float m12, float m13,
00032                                                                                                 float m20, float m21, float m22, float m23,
00033                                                                                                 float m30, float m31, float m32, float m33)
00034                                                                         {
00035                                                                                 m[0][0] = m00;  m[0][1] = m01;  m[0][2] = m02;  m[0][3] = m03;
00036                                                                                 m[1][0] = m10;  m[1][1] = m11;  m[1][2] = m12;  m[1][3] = m13;
00037                                                                                 m[2][0] = m20;  m[2][1] = m21;  m[2][2] = m22;  m[2][3] = m23;
00038                                                                                 m[3][0] = m30;  m[3][1] = m31;  m[3][2] = m32;  m[3][3] = m33;
00039                                                                         }
00041                 inline_                                         Matrix4x4(const Matrix4x4& mat)                         { CopyMemory(m, &mat.m, 16*sizeof(float));      }
00043                 inline_                                         ~Matrix4x4()                                                            {}
00044 
00046                 inline_ Matrix4x4&                      Set(    float m00, float m01, float m02,
00047                                                                                         float m10, float m11, float m12,
00048                                                                                         float m20, float m21, float m22)
00049                                                                         {
00050                                                                                 m[0][0] = m00;  m[0][1] = m01;  m[0][2] = m02;
00051                                                                                 m[1][0] = m10;  m[1][1] = m11;  m[1][2] = m12;
00052                                                                                 m[2][0] = m20;  m[2][1] = m21;  m[2][2] = m22;
00053                                                                                 return *this;
00054                                                                         }
00056                 inline_ Matrix4x4&                      Set(    float m00, float m01, float m02, float m03,
00057                                                                                         float m10, float m11, float m12, float m13,
00058                                                                                         float m20, float m21, float m22, float m23,
00059                                                                                         float m30, float m31, float m32, float m33)
00060                                                                         {
00061                                                                                 m[0][0] = m00;  m[0][1] = m01;  m[0][2] = m02;  m[0][3] = m03;
00062                                                                                 m[1][0] = m10;  m[1][1] = m11;  m[1][2] = m12;  m[1][3] = m13;
00063                                                                                 m[2][0] = m20;  m[2][1] = m21;  m[2][2] = m22;  m[2][3] = m23;
00064                                                                                 m[3][0] = m30;  m[3][1] = m31;  m[3][2] = m32;  m[3][3] = m33;
00065                                                                                 return *this;
00066                                                                         }
00067 
00069                 inline_ void                            Copy(const Matrix4x4& source)                           { CopyMemory(m, source.m, 16*sizeof(float));    }
00070 
00071                 // Row-column access
00073                 inline_ void                            GetRow(const udword r, HPoint& p)       const   { p.x=m[r][0];  p.y=m[r][1];    p.z=m[r][2];    p.w=m[r][3];            }
00075                 inline_ void                            GetRow(const udword r, Point& p)        const   { p.x=m[r][0];  p.y=m[r][1];    p.z=m[r][2];                                            }
00077                 inline_ const HPoint&           GetRow(const udword r)                          const   { return *(const HPoint*)&m[r][0];                                                                      }
00079                 inline_ HPoint&                         GetRow(const udword r)                                          { return *(HPoint*)&m[r][0];                                                                            }
00081                 inline_ void                            SetRow(const udword r, const HPoint& p)         { m[r][0]=p.x;  m[r][1]=p.y;    m[r][2]=p.z;    m[r][3]=p.w;            }
00083                 inline_ void                            SetRow(const udword r, const Point& p)          { m[r][0]=p.x;  m[r][1]=p.y;    m[r][2]=p.z;    m[r][3]= (r!=3) ? 0.0f : 1.0f;          }
00085                 inline_ void                            GetCol(const udword c, HPoint& p)       const   { p.x=m[0][c];  p.y=m[1][c];    p.z=m[2][c];    p.w=m[3][c];            }
00087                 inline_ void                            GetCol(const udword c, Point& p)        const   { p.x=m[0][c];  p.y=m[1][c];    p.z=m[2][c];                                            }
00089                 inline_ void                            SetCol(const udword c, const HPoint& p)         { m[0][c]=p.x;  m[1][c]=p.y;    m[2][c]=p.z;    m[3][c]=p.w;            }
00091                 inline_ void                            SetCol(const udword c, const Point& p)          { m[0][c]=p.x;  m[1][c]=p.y;    m[2][c]=p.z;    m[3][c]= (c!=3) ? 0.0f : 1.0f;  }
00092 
00093                 // Translation
00095                 inline_ const HPoint&           GetTrans()                                                      const   { return GetRow(3);                                                             }
00097                 inline_ void                            GetTrans(Point& p)                                      const   { p.x=m[3][0];  p.y=m[3][1];    p.z=m[3][2];    }
00099                 inline_ void                            SetTrans(const Point& p)                                        { m[3][0]=p.x;  m[3][1]=p.y;    m[3][2]=p.z;    }
00101                 inline_ void                            SetTrans(const HPoint& p)                                       { m[3][0]=p.x;  m[3][1]=p.y;    m[3][2]=p.z;    m[3][3]=p.w;    }
00103                 inline_ void                            SetTrans(float tx, float ty, float tz)          { m[3][0]=tx;   m[3][1]=ty;             m[3][2]=tz;             }
00104 
00105                 // Scale
00107                 inline_ void                            SetScale(const Point& p)                                        { m[0][0]=p.x;  m[1][1]=p.y;    m[2][2]=p.z;    }
00109                 inline_ void                            SetScale(float sx, float sy, float sz)          { m[0][0]=sx;   m[1][1]=sy;             m[2][2]=sz;             }
00111                                 void                            Scale(const Point& p)
00112                                                                         {
00113                                                                                 m[0][0] *= p.x; m[1][0] *= p.y; m[2][0] *= p.z;
00114                                                                                 m[0][1] *= p.x; m[1][1] *= p.y; m[2][1] *= p.z;
00115                                                                                 m[0][2] *= p.x; m[1][2] *= p.y; m[2][2] *= p.z;
00116                                                                         }
00118                                 void                            Scale(float sx, float sy, float sz)
00119                                                                         {
00120                                                                                 m[0][0] *= sx;  m[1][0] *= sy;  m[2][0] *= sz;
00121                                                                                 m[0][1] *= sx;  m[1][1] *= sy;  m[2][1] *= sz;
00122                                                                                 m[0][2] *= sx;  m[1][2] *= sy;  m[2][2] *= sz;
00123                                                                         }
00124 /*
00126                 inline_ HPoint          GetRow(const udword row)        const                   { return mRow[row];                                                                                                             }
00128                 inline_ Matrix4x4&      SetRow(const udword row, const HPoint& p)       { mRow[row] = p;        return *this;                                                                           }
00130                                                 Matrix4x4&      SetRow(const udword row, const Point& p)
00131                                                 {
00132                                                         m[row][0] = p.x;
00133                                                         m[row][1] = p.y;
00134                                                         m[row][2] = p.z;
00135                                                         m[row][3] = (row != 3) ? 0.0f : 1.0f;
00136                                                         return  *this;
00137                                                 }
00139                                                 HPoint          GetCol(const udword col)                const
00140                                                 {
00141                                                         HPoint  Res;
00142                                                         Res.x = m[0][col];
00143                                                         Res.y = m[1][col];
00144                                                         Res.z = m[2][col];
00145                                                         Res.w = m[3][col];
00146                                                         return  Res;
00147                                                 }
00149                                                 Matrix4x4&      SetCol(const udword col, const HPoint& p)
00150                                                 {
00151                                                         m[0][col] = p.x;
00152                                                         m[1][col] = p.y;
00153                                                         m[2][col] = p.z;
00154                                                         m[3][col] = p.w;
00155                                                         return  *this;
00156                                                 }
00158                                                 Matrix4x4&      SetCol(const udword col, const Point& p)
00159                                                 {
00160                                                         m[0][col] = p.x;
00161                                                         m[1][col] = p.y;
00162                                                         m[2][col] = p.z;
00163                                                         m[3][col] = (col != 3) ? 0.0f : 1.0f;
00164                                                         return  *this;
00165                                                 }
00166 */
00168                 inline_ float                           Trace()                 const                   { return m[0][0] + m[1][1] + m[2][2] + m[3][3];                 }
00170                 inline_ float                           Trace3x3()              const                   { return m[0][0] + m[1][1] + m[2][2];                                   }
00172                 inline_ void                            Zero()                                                  { ZeroMemory(&m,  sizeof(m));                                                   }
00174                 inline_ void                            Identity()                                              { Zero(); m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0f; }
00176                 inline_ bool                            IsIdentity()    const
00177                                                                         {
00178                                                                                 if(IR(m[0][0])!=IEEE_1_0)       return false;
00179                                                                                 if(IR(m[0][1])!=0)                      return false;
00180                                                                                 if(IR(m[0][2])!=0)                      return false;
00181                                                                                 if(IR(m[0][3])!=0)                      return false;
00182 
00183                                                                                 if(IR(m[1][0])!=0)                      return false;
00184                                                                                 if(IR(m[1][1])!=IEEE_1_0)       return false;
00185                                                                                 if(IR(m[1][2])!=0)                      return false;
00186                                                                                 if(IR(m[1][3])!=0)                      return false;
00187 
00188                                                                                 if(IR(m[2][0])!=0)                      return false;
00189                                                                                 if(IR(m[2][1])!=0)                      return false;
00190                                                                                 if(IR(m[2][2])!=IEEE_1_0)       return false;
00191                                                                                 if(IR(m[2][3])!=0)                      return false;
00192 
00193                                                                                 if(IR(m[3][0])!=0)                      return false;
00194                                                                                 if(IR(m[3][1])!=0)                      return false;
00195                                                                                 if(IR(m[3][2])!=0)                      return false;
00196                                                                                 if(IR(m[3][3])!=IEEE_1_0)       return false;
00197                                                                                 return true;
00198                                                                         }
00199 
00201                 inline_ BOOL                            IsValid()               const
00202                                                                         {
00203                                                                                 for(udword j=0;j<4;j++)
00204                                                                                 {
00205                                                                                         for(udword i=0;i<4;i++)
00206                                                                                         {
00207                                                                                                 if(!IsValidFloat(m[j][i]))      return FALSE;
00208                                                                                         }
00209                                                                                 }
00210                                                                                 return TRUE;
00211                                                                         }
00212 
00214                                 void                            RotX(float angle)       { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[1][1] = m[2][2] = Cos; m[2][1] = -Sin;      m[1][2] = Sin;  }
00216                                 void                            RotY(float angle)       { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[2][2] = Cos; m[2][0] = Sin;       m[0][2] = -Sin; }
00218                                 void                            RotZ(float angle)       { float Cos = cosf(angle), Sin = sinf(angle); Identity(); m[0][0] = m[1][1] = Cos; m[1][0] = -Sin;      m[0][1] = Sin;  }
00219 
00221                                 Matrix4x4&                      Rot(float angle, Point& p1, Point& p2);
00222 
00224                                 void                            Transpose()
00225                                                                         {
00226                                                                                 IR(m[1][0]) ^= IR(m[0][1]);             IR(m[0][1]) ^= IR(m[1][0]);             IR(m[1][0]) ^= IR(m[0][1]);
00227                                                                                 IR(m[2][0]) ^= IR(m[0][2]);             IR(m[0][2]) ^= IR(m[2][0]);             IR(m[2][0]) ^= IR(m[0][2]);
00228                                                                                 IR(m[3][0]) ^= IR(m[0][3]);             IR(m[0][3]) ^= IR(m[3][0]);             IR(m[3][0]) ^= IR(m[0][3]);
00229                                                                                 IR(m[1][2]) ^= IR(m[2][1]);             IR(m[2][1]) ^= IR(m[1][2]);             IR(m[1][2]) ^= IR(m[2][1]);
00230                                                                                 IR(m[1][3]) ^= IR(m[3][1]);             IR(m[3][1]) ^= IR(m[1][3]);             IR(m[1][3]) ^= IR(m[3][1]);
00231                                                                                 IR(m[2][3]) ^= IR(m[3][2]);             IR(m[3][2]) ^= IR(m[2][3]);             IR(m[2][3]) ^= IR(m[3][2]);
00232                                                                         }
00233 
00235                                 float                           CoFactor(udword row, udword col)        const;
00237                                 float                           Determinant()   const;
00239                                 Matrix4x4&                      Invert();
00240 //                              Matrix& ComputeAxisMatrix(Point& axis, float angle);
00241 
00242                 // Cast operators
00244                 inline_ operator                        Matrix3x3()     const
00245                                                                         {
00246                                                                                 return Matrix3x3(
00247                                                                                 m[0][0],        m[0][1],        m[0][2],
00248                                                                                 m[1][0],        m[1][1],        m[1][2],
00249                                                                                 m[2][0],        m[2][1],        m[2][2]);
00250                                                                         }
00252                                 operator                        Quat()  const;
00254                                 operator                        PR()    const;
00255 
00256                 // Arithmetic operators
00258                 inline_ Matrix4x4                       operator+(const Matrix4x4& mat) const
00259                                                                         {
00260                                                                                 return Matrix4x4(
00261                                                                                 m[0][0]+mat.m[0][0], m[0][1]+mat.m[0][1], m[0][2]+mat.m[0][2], m[0][3]+mat.m[0][3], 
00262                                                                                 m[1][0]+mat.m[1][0], m[1][1]+mat.m[1][1], m[1][2]+mat.m[1][2], m[1][3]+mat.m[1][3], 
00263                                                                                 m[2][0]+mat.m[2][0], m[2][1]+mat.m[2][1], m[2][2]+mat.m[2][2], m[2][3]+mat.m[2][3], 
00264                                                                                 m[3][0]+mat.m[3][0], m[3][1]+mat.m[3][1], m[3][2]+mat.m[3][2], m[3][3]+mat.m[3][3]);
00265                                                                         }
00266 
00268                 inline_ Matrix4x4                       operator-(const Matrix4x4& mat) const
00269                                                                         {
00270                                                                                 return Matrix4x4(
00271                                                                                 m[0][0]-mat.m[0][0], m[0][1]-mat.m[0][1], m[0][2]-mat.m[0][2], m[0][3]-mat.m[0][3], 
00272                                                                                 m[1][0]-mat.m[1][0], m[1][1]-mat.m[1][1], m[1][2]-mat.m[1][2], m[1][3]-mat.m[1][3], 
00273                                                                                 m[2][0]-mat.m[2][0], m[2][1]-mat.m[2][1], m[2][2]-mat.m[2][2], m[2][3]-mat.m[2][3], 
00274                                                                                 m[3][0]-mat.m[3][0], m[3][1]-mat.m[3][1], m[3][2]-mat.m[3][2], m[3][3]-mat.m[3][3]);
00275                                                                         }
00276 
00278                 inline_ Matrix4x4                       operator*(const Matrix4x4& mat) const
00279                                                                         {
00280                                                                                 return Matrix4x4(
00281                                                                                 m[0][0]*mat.m[0][0] + m[0][1]*mat.m[1][0] + m[0][2]*mat.m[2][0] + m[0][3]*mat.m[3][0],
00282                                                                                 m[0][0]*mat.m[0][1] + m[0][1]*mat.m[1][1] + m[0][2]*mat.m[2][1] + m[0][3]*mat.m[3][1],
00283                                                                                 m[0][0]*mat.m[0][2] + m[0][1]*mat.m[1][2] + m[0][2]*mat.m[2][2] + m[0][3]*mat.m[3][2],
00284                                                                                 m[0][0]*mat.m[0][3] + m[0][1]*mat.m[1][3] + m[0][2]*mat.m[2][3] + m[0][3]*mat.m[3][3],
00285 
00286                                                                                 m[1][0]*mat.m[0][0] + m[1][1]*mat.m[1][0] + m[1][2]*mat.m[2][0] + m[1][3]*mat.m[3][0],
00287                                                                                 m[1][0]*mat.m[0][1] + m[1][1]*mat.m[1][1] + m[1][2]*mat.m[2][1] + m[1][3]*mat.m[3][1],
00288                                                                                 m[1][0]*mat.m[0][2] + m[1][1]*mat.m[1][2] + m[1][2]*mat.m[2][2] + m[1][3]*mat.m[3][2],
00289                                                                                 m[1][0]*mat.m[0][3] + m[1][1]*mat.m[1][3] + m[1][2]*mat.m[2][3] + m[1][3]*mat.m[3][3],
00290 
00291                                                                                 m[2][0]*mat.m[0][0] + m[2][1]*mat.m[1][0] + m[2][2]*mat.m[2][0] + m[2][3]*mat.m[3][0],
00292                                                                                 m[2][0]*mat.m[0][1] + m[2][1]*mat.m[1][1] + m[2][2]*mat.m[2][1] + m[2][3]*mat.m[3][1],
00293                                                                                 m[2][0]*mat.m[0][2] + m[2][1]*mat.m[1][2] + m[2][2]*mat.m[2][2] + m[2][3]*mat.m[3][2],
00294                                                                                 m[2][0]*mat.m[0][3] + m[2][1]*mat.m[1][3] + m[2][2]*mat.m[2][3] + m[2][3]*mat.m[3][3],
00295 
00296                                                                                 m[3][0]*mat.m[0][0] + m[3][1]*mat.m[1][0] + m[3][2]*mat.m[2][0] + m[3][3]*mat.m[3][0],
00297                                                                                 m[3][0]*mat.m[0][1] + m[3][1]*mat.m[1][1] + m[3][2]*mat.m[2][1] + m[3][3]*mat.m[3][1],
00298                                                                                 m[3][0]*mat.m[0][2] + m[3][1]*mat.m[1][2] + m[3][2]*mat.m[2][2] + m[3][3]*mat.m[3][2],
00299                                                                                 m[3][0]*mat.m[0][3] + m[3][1]*mat.m[1][3] + m[3][2]*mat.m[2][3] + m[3][3]*mat.m[3][3]);
00300                                                                         }
00301 
00303                 inline_ HPoint                          operator*(const HPoint& v)              const   { return HPoint(GetRow(0)|v, GetRow(1)|v, GetRow(2)|v, GetRow(3)|v); }
00304 
00306                 inline_ Point                           operator*(const Point& v)               const
00307                                                                         {
00308                                                                                 return Point(   m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z + m[0][3],
00309                                                                                                                 m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z + m[1][3],
00310                                                                                                                 m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z + m[2][3]       );
00311                                                                         }
00312 
00314                 inline_ Matrix4x4                       operator*(float s)                              const
00315                                                                         {
00316                                                                                 return Matrix4x4(
00317                                                                                 m[0][0]*s,      m[0][1]*s,      m[0][2]*s,      m[0][3]*s,
00318                                                                                 m[1][0]*s,      m[1][1]*s,      m[1][2]*s,      m[1][3]*s,
00319                                                                                 m[2][0]*s,      m[2][1]*s,      m[2][2]*s,      m[2][3]*s,
00320                                                                                 m[3][0]*s,      m[3][1]*s,      m[3][2]*s,      m[3][3]*s);
00321                                                                         }
00322 
00324                 inline_ friend Matrix4x4        operator*(float s, const Matrix4x4& mat)
00325                                                                         {
00326                                                                                 return Matrix4x4(
00327                                                                                 s*mat.m[0][0],  s*mat.m[0][1],  s*mat.m[0][2],  s*mat.m[0][3],
00328                                                                                 s*mat.m[1][0],  s*mat.m[1][1],  s*mat.m[1][2],  s*mat.m[1][3],
00329                                                                                 s*mat.m[2][0],  s*mat.m[2][1],  s*mat.m[2][2],  s*mat.m[2][3],
00330                                                                                 s*mat.m[3][0],  s*mat.m[3][1],  s*mat.m[3][2],  s*mat.m[3][3]);
00331                                                                         }
00332 
00334                 inline_ Matrix4x4                       operator/(float s)                              const
00335                                                                         {
00336                                                                                 if(s) s = 1.0f / s;
00337 
00338                                                                                 return Matrix4x4(
00339                                                                                 m[0][0]*s,      m[0][1]*s,      m[0][2]*s,      m[0][3]*s,
00340                                                                                 m[1][0]*s,      m[1][1]*s,      m[1][2]*s,      m[1][3]*s,
00341                                                                                 m[2][0]*s,      m[2][1]*s,      m[2][2]*s,      m[2][3]*s,
00342                                                                                 m[3][0]*s,      m[3][1]*s,      m[3][2]*s,      m[3][3]*s);
00343                                                                         }
00344 
00346                 inline_ friend Matrix4x4        operator/(float s, const Matrix4x4& mat)
00347                                                                         {
00348                                                                                 return Matrix4x4(
00349                                                                                 s/mat.m[0][0],  s/mat.m[0][1],  s/mat.m[0][2],  s/mat.m[0][3],
00350                                                                                 s/mat.m[1][0],  s/mat.m[1][1],  s/mat.m[1][2],  s/mat.m[1][3],
00351                                                                                 s/mat.m[2][0],  s/mat.m[2][1],  s/mat.m[2][2],  s/mat.m[2][3],
00352                                                                                 s/mat.m[3][0],  s/mat.m[3][1],  s/mat.m[3][2],  s/mat.m[3][3]);
00353                                                                         }
00354 
00356                 inline_ Matrix4x4&                      operator+=(const Matrix4x4& mat)
00357                                                                         {
00358                                                                                 m[0][0]+=mat.m[0][0];   m[0][1]+=mat.m[0][1];   m[0][2]+=mat.m[0][2];   m[0][3]+=mat.m[0][3];
00359                                                                                 m[1][0]+=mat.m[1][0];   m[1][1]+=mat.m[1][1];   m[1][2]+=mat.m[1][2];   m[1][3]+=mat.m[1][3];
00360                                                                                 m[2][0]+=mat.m[2][0];   m[2][1]+=mat.m[2][1];   m[2][2]+=mat.m[2][2];   m[2][3]+=mat.m[2][3];
00361                                                                                 m[3][0]+=mat.m[3][0];   m[3][1]+=mat.m[3][1];   m[3][2]+=mat.m[3][2];   m[3][3]+=mat.m[3][3];
00362                                                                                 return  *this;
00363                                                                         }
00364 
00366                 inline_ Matrix4x4&                      operator-=(const Matrix4x4& mat)
00367                                                                         {
00368                                                                                 m[0][0]-=mat.m[0][0];   m[0][1]-=mat.m[0][1];   m[0][2]-=mat.m[0][2];   m[0][3]-=mat.m[0][3];
00369                                                                                 m[1][0]-=mat.m[1][0];   m[1][1]-=mat.m[1][1];   m[1][2]-=mat.m[1][2];   m[1][3]-=mat.m[1][3];
00370                                                                                 m[2][0]-=mat.m[2][0];   m[2][1]-=mat.m[2][1];   m[2][2]-=mat.m[2][2];   m[2][3]-=mat.m[2][3];
00371                                                                                 m[3][0]-=mat.m[3][0];   m[3][1]-=mat.m[3][1];   m[3][2]-=mat.m[3][2];   m[3][3]-=mat.m[3][3];
00372                                                                                 return  *this;
00373                                                                         }
00374 
00376                                 Matrix4x4&                      operator*=(const Matrix4x4& mat)
00377                                                                         {
00378                                                                                 HPoint TempRow;
00379 
00380                                                                                 GetRow(0, TempRow);
00381                                                                                 m[0][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
00382                                                                                 m[0][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
00383                                                                                 m[0][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
00384                                                                                 m[0][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
00385 
00386                                                                                 GetRow(1, TempRow);
00387                                                                                 m[1][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
00388                                                                                 m[1][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
00389                                                                                 m[1][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
00390                                                                                 m[1][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
00391 
00392                                                                                 GetRow(2, TempRow);
00393                                                                                 m[2][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
00394                                                                                 m[2][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
00395                                                                                 m[2][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
00396                                                                                 m[2][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
00397 
00398                                                                                 GetRow(3, TempRow);
00399                                                                                 m[3][0] = TempRow.x*mat.m[0][0] + TempRow.y*mat.m[1][0] + TempRow.z*mat.m[2][0] + TempRow.w*mat.m[3][0];
00400                                                                                 m[3][1] = TempRow.x*mat.m[0][1] + TempRow.y*mat.m[1][1] + TempRow.z*mat.m[2][1] + TempRow.w*mat.m[3][1];
00401                                                                                 m[3][2] = TempRow.x*mat.m[0][2] + TempRow.y*mat.m[1][2] + TempRow.z*mat.m[2][2] + TempRow.w*mat.m[3][2];
00402                                                                                 m[3][3] = TempRow.x*mat.m[0][3] + TempRow.y*mat.m[1][3] + TempRow.z*mat.m[2][3] + TempRow.w*mat.m[3][3];
00403 
00404                                                                                 return  *this;
00405                                                                         }
00406 
00408                 inline_ Matrix4x4&              operator*=(float s)
00409                                                                 {
00410                                                                         m[0][0]*=s;     m[0][1]*=s;     m[0][2]*=s;     m[0][3]*=s;
00411                                                                         m[1][0]*=s;     m[1][1]*=s;     m[1][2]*=s;     m[1][3]*=s;
00412                                                                         m[2][0]*=s;     m[2][1]*=s;     m[2][2]*=s;     m[2][3]*=s;
00413                                                                         m[3][0]*=s;     m[3][1]*=s;     m[3][2]*=s;     m[3][3]*=s;
00414                                                                         return  *this;
00415                                                                 }
00416 
00418                 inline_ Matrix4x4&              operator/=(float s)
00419                                                                 {
00420                                                                         if(s)  s = 1.0f / s;
00421                                                                         m[0][0]*=s;     m[0][1]*=s;     m[0][2]*=s;     m[0][3]*=s;
00422                                                                         m[1][0]*=s;     m[1][1]*=s;     m[1][2]*=s;     m[1][3]*=s;
00423                                                                         m[2][0]*=s;     m[2][1]*=s;     m[2][2]*=s;     m[2][3]*=s;
00424                                                                         m[3][0]*=s;     m[3][1]*=s;     m[3][2]*=s;     m[3][3]*=s;
00425                                                                         return  *this;
00426                                                                 }
00427 
00428                 inline_ const HPoint&   operator[](int row)             const   { return *(const HPoint*)&m[row][0];    }
00429                 inline_ HPoint&                 operator[](int row)                             { return *(HPoint*)&m[row][0];                  }
00430 
00431                 public:
00432 
00433                                 float                   m[4][4];
00434         };
00435 
00437         inline_ void TransformPoint4x3(Point& dest, const Point& source, const Matrix4x4& rot)
00438         {
00439                 dest.x = rot.m[3][0] + source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
00440                 dest.y = rot.m[3][1] + source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
00441                 dest.z = rot.m[3][2] + source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
00442         }
00443 
00445         inline_ void TransformPoint3x3(Point& dest, const Point& source, const Matrix4x4& rot)
00446         {
00447                 dest.x = source.x * rot.m[0][0] + source.y * rot.m[1][0] + source.z * rot.m[2][0];
00448                 dest.y = source.x * rot.m[0][1] + source.y * rot.m[1][1] + source.z * rot.m[2][1];
00449                 dest.z = source.x * rot.m[0][2] + source.y * rot.m[1][2] + source.z * rot.m[2][2];
00450         }
00451 
00452         ICEMATHS_API void InvertPRMatrix(Matrix4x4& dest, const Matrix4x4& src);
00453 
00454 #endif // __ICEMATRIX4X4_H__
00455 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54