Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/Math/Matrix.h

Go to the documentation of this file.

    00001 /**
    00002  * \file Matrix.h
    00003  * Template classes for various 3x3 matrices.
    00004  *
    00005  * \author Martin Kallnik, martin.kallnik@gmx.de
    00006  * \author Thomas Kindler, thomas.kindler@gmx.de
    00007  * \author Max Risler
    00008  */
    00009
    00010 #ifndef __Matrix_h__
    00011 #define __Matrix_h__
    00012
    00013 #include "Vector3.h"
    00014
    00015 /**
    00016  * This class represents a 3x3-matrix
    00017  *
    00018  */
    00019 template <class V> class Matrix3x3 {
    00020 public:
    00021   /**
    00022    * The columns of the matrix
    00023    */
    00024   Vector3<V> c[3];
    00025
    00026   /**
    00027    * Default constructor.
    00028    */
    00029   Matrix3x3<V>()
    00030   {
    00031     c[0]=Vector3<V>(1,0,0);
    00032     c[1]=Vector3<V>(0,1,0);
    00033     c[2]=Vector3<V>(0,0,1);
    00034   }
    00035
    00036   /**
    00037    * Constructor.
    00038    *
    00039    * \param  c0  the first column of the matrix.
    00040    * \param  c1  the second column of the matrix.
    00041    * \param  c2  the third column of the matrix.
    00042   */
    00043   Matrix3x3<V>(
    00044     const Vector3<V>& c0,
    00045     const Vector3<V>& c1,
    00046     const Vector3<V>& c2
    00047   )
    00048   {
    00049     c[0] = c0;
    00050     c[1] = c1;
    00051     c[2] = c2;
    00052   }
    00053
    00054   /**
    00055    * Assignment operator.
    00056    *
    00057    * \param  other   The other matrix that is assigned to this one
    00058    * \return         A reference to this object after the assignment.
    00059   */
    00060   Matrix3x3<V>& operator=(const Matrix3x3<V>& other)
    00061   {
    00062     c[0] = other.c[0];
    00063     c[1] = other.c[1];
    00064     c[2] = other.c[2];
    00065     return *this;
    00066   }
    00067
    00068   /**
    00069    * Copy constructor.
    00070    *
    00071    * \param other The other matrix that is copied to this one
    00072    */
    00073   Matrix3x3<V>(const Matrix3x3<V>& other)
    00074   {
    00075     *this = other;
    00076   }
    00077
    00078   /**
    00079    * Adds this matrix with another matrix.
    00080    *
    00081    * \param  other  The matrix this one is added to
    00082    * \return         A new vector containing the result
    00083    *                 of the calculation.
    00084   */
    00085   Matrix3x3<V> operator+(const Matrix3x3<V>& other) const
    00086   {
    00087     return Matrix3x3<V>(
    00088       (*this).c[0]+other.c[0],
    00089       (*this).c[1]+other.c[1],
    00090       (*this).c[2]+other.c[2]
    00091     );
    00092   }
    00093    /**
    00094    * Adds this matrix to another matrix.
    00095    *
    00096    * \param  other  The other matrix that is added to this one
    00097    * \return        A reference this object after the calculation.
    00098   */
    00099   Matrix3x3<V>& operator+=(const Matrix3x3<V>& other)
    00100   {
    00101     (*this).c[0]+=other.c[0];
    00102     (*this).c[1]+=other.c[1];
    00103     (*this).c[2]+=other.c[2];
    00104     return *this;
    00105   }
    00106   /**
    00107    * Multiplication of this matrix by vector.
    00108    *
    00109    * \param  vector  The vector this one is multiplied by
    00110    * \return         A new vector containing the result
    00111    *                 of the calculation.
    00112   */
    00113   Vector3<V> operator*(const Vector3<V>& vector) const
    00114   {
    00115     return (c[0]*vector.x + c[1]*vector.y + c[2]*vector.z);
    00116   }
    00117
    00118   /**
    00119    * Multiplication of this matrix by another matrix.
    00120    *
    00121    * \param  other  The other matrix this one is multiplied by
    00122    * \return        A new matrix containing the result
    00123    *                of the calculation.
    00124   */
    00125   Matrix3x3<V> operator*(const Matrix3x3<V>& other) const
    00126   {
    00127     return Matrix3x3<V>(
    00128       (*this)*other.c[0],
    00129       (*this)*other.c[1],
    00130       (*this)*other.c[2]
    00131     );
    00132   }
    00133
    00134   /**
    00135    * Multiplication of this matrix by another matrix.
    00136    *
    00137    * \param  other  The other matrix this one is multiplied by
    00138    * \return        A reference this object after the calculation.
    00139   */
    00140   Matrix3x3<V>& operator*=(const Matrix3x3<V>& other)
    00141   {
    00142     return *this = *this * other;
    00143   }
    00144
    00145   /**
    00146    * Multiplication of this matrix by a factor.
    00147    *
    00148    * \param  factor  The factor this matrix is multiplied by
    00149    * \return         A reference to this object after the calculation.
    00150   */
    00151   Matrix3x3<V>& operator*=(const V& factor)
    00152   {
    00153     c[0] *= factor;
    00154     c[1] *= factor;
    00155     c[2] *= factor;
    00156     return *this;
    00157   }
    00158
    00159   /**
    00160    * Division of this matrix by a factor.
    00161    *
    00162    * \param  factor  The factor this matrix is divided by
    00163    * \return         A reference to this object after the calculation.
    00164    */
    00165   Matrix3x3<V>& operator/=(const V& factor)
    00166   {
    00167     *this *= 1 / factor;
    00168     return *this;
    00169   }
    00170
    00171   /**
    00172    * Multiplication of this matrix by a factor.
    00173    *
    00174    * \param  factor  The factor this matrix is multiplied by
    00175    * \return         A new object that contains the result of the calculation.
    00176    */
    00177   Matrix3x3<V> operator*(const V& factor) const
    00178   {
    00179     return Matrix3x3<V>(*this) *= factor;
    00180   }
    00181
    00182   /**
    00183    * Division of this matrix by a factor.
    00184    *
    00185    * \param  factor  The factor this matrix is divided by
    00186    * \return         A new object that contains the result of the calculation.
    00187    */
    00188   Matrix3x3<V> operator/(const V& factor) const
    00189   {
    00190     return Matrix3x3<V>(*this) /= factor;
    00191   }
    00192
    00193   /**
    00194    * Comparison of another matrix with this one.
    00195    *
    00196    * \param  other  The other matrix that will be compared to this one
    00197    * \return        Whether the two matrices are equal.
    00198    */
    00199   bool operator==(const Matrix3x3<V>& other) const
    00200   {
    00201     return (
    00202       c[0] == other.c[0] &&
    00203       c[1] == other.c[1] &&
    00204       c[2] == other.c[2]
    00205     );
    00206   }
    00207
    00208   /**
    00209    * Comparison of another matrix with this one.
    00210    *
    00211    * \param  other  The other matrix that will be compared to this one
    00212    * \return        Whether the two matrixs are unequal.
    00213    */
    00214   bool operator!=(const Matrix3x3<V>& other) const
    00215   {
    00216     return !(*this == other);
    00217   }
    00218
    00219   /**
    00220    * Array-like member access.
    00221    * \param  i index
    00222    * \return reference to column
    00223    */
    00224   Vector3<V>& operator[](int i)
    00225   {
    00226     return c[i];
    00227   }
    00228
    00229   /**
    00230    * Transpose the matrix
    00231    *
    00232    * \return  A new object containing transposed matrix
    00233    */
    00234   Matrix3x3<V> transpose() const
    00235   {
    00236     return Matrix3x3<V>(
    00237       Vector3<V>(c[0].x, c[1].x, c[2].x),
    00238       Vector3<V>(c[0].y, c[1].y, c[2].y),
    00239       Vector3<V>(c[0].z, c[1].z, c[2].z)
    00240     );
    00241   }
    00242
    00243   /**
    00244    * Calculation of the determinant of this matrix.
    00245    *
    00246    * \return The determinant.
    00247    */
    00248   V det() const
    00249   {
    00250     return
    00251       c[0].x * (c[1].y * c[2].z - c[1].z * c[2].y) +
    00252       c[0].y * (c[1].z * c[2].x - c[1].x * c[2].z) +
    00253       c[0].z * (c[1].x * c[2].y - c[1].y * c[2].x);
    00254   }
    00255
    00256   /**
    00257    * Calculate determinant of 2x2 Submatrix
    00258    * | a b |
    00259    * | c d |
    00260    *
    00261    * \return  determinant.
    00262    */
    00263   static V det2(V a, V b, V c, V d)
    00264   {
    00265     return a*d - b*c;
    00266   }
    00267
    00268   /**
    00269    * Calculate the adjoint of this matrix.
    00270    *
    00271    * \return the adjoint matrix.
    00272    */
    00273   Matrix3x3<V> adjoint() const
    00274   {
    00275     return Matrix3x3<V>(
    00276       Vector3<V>(
    00277         det2(c[1].y, c[2].y, c[1].z, c[2].z),
    00278         det2(c[2].x, c[1].x, c[2].z, c[1].z),
    00279         det2(c[1].x, c[2].x, c[1].y, c[2].y)
    00280       ),
    00281       Vector3<V>(
    00282         det2(c[2].y, c[0].y, c[2].z, c[0].z),
    00283         det2(c[0].x, c[2].x, c[0].z, c[2].z),
    00284         det2(c[2].x, c[0].x, c[2].y, c[0].y)
    00285       ),
    00286       Vector3<V>(
    00287         det2(c[0].y, c[1].y, c[0].z, c[1].z),
    00288         det2(c[1].x, c[0].x, c[1].z, c[0].z),
    00289         det2(c[0].x, c[1].x, c[0].y, c[1].y)
    00290       )
    00291     );
    00292
    00293   }
    00294
    00295   /**
    00296    * Calculate the inverse of this matrix.
    00297    *
    00298    * \return The inverse matrix
    00299    */
    00300   Matrix3x3<V> invert() const
    00301   {
    00302     return adjoint().transpose() / det();
    00303   }
    00304 };
    00305
    00306
    00307 /**
    00308  * Streaming operator that reads a Matrix3x3<V> from a stream.
    00309  *
    00310  * \param   stream     The stream from which is read.
    00311  * \param   matrix3x3  The Matrix3x3<V> object.
    00312  * \return  The stream.
    00313  */
    00314 template <class V> In& operator>>(In& stream, Matrix3x3<V>& matrix3x3);
    00315
    00316
    00317 /**
    00318  * Streaming operator that writes a Matrix3x3<V> to a stream.
    00319  *
    00320  * \param   stream      The stream to write on.
    00321  * \param   matrix3x3   The Matrix3x3<V> object.
    00322  * \return  The stream.
    00323  */
    00324 template <class V> Out& operator<<(Out& stream, const Matrix3x3<V>& matrix3x3);
    00325
    00326
    00327 /**
    00328  * Representation for 3x3 RotationMatrices
    00329  */
    00330 class RotationMatrix : public Matrix3x3<double> {
    00331 public:
    00332   /**
    00333    * Default constructor.
    00334    */
    00335   RotationMatrix() {}
    00336
    00337   /**
    00338    * Constructor.
    00339    *
    00340    * \param  c0  the first column of the matrix.
    00341    * \param  c1  the second column of the matrix.
    00342    * \param  c2  the third column of the matrix.
    00343    */
    00344   RotationMatrix(
    00345     const Vector3<double>& c0,
    00346     const Vector3<double>& c1,
    00347     const Vector3<double>& c2
    00348   )
    00349   : Matrix3x3<double>(c0,c1,c2)
    00350   {
    00351   }
    00352
    00353   /**
    00354    * Assignment operator.
    00355    *
    00356    * \param  other  The other matrix that is assigned to this one
    00357    * \return        A reference to this object after the assignment.
    00358    */
    00359   RotationMatrix& operator=(const Matrix3x3<double>& other)
    00360   {
    00361     c[0] = other.c[0];
    00362     c[1] = other.c[1];
    00363     c[2] = other.c[2];
    00364     return *this;
    00365   }
    00366
    00367   /**
    00368    * Copy constructor.
    00369    *
    00370    * \param  other  The other matrix that is copied to this one
    00371    */
    00372   RotationMatrix(const Matrix3x3<double>& other)
    00373   {
    00374     *this = other;
    00375   }
    00376
    00377   /**
    00378    * RotationMatrix from RPY-angles.
    00379    *   Roll  rotates along z axis,
    00380    *   Pitch rotates along y axis,
    00381    *   Yaw   rotates along x axis
    00382    *
    00383    *   R(roll,pitch,yaw) = R(z,roll)*R(y,pitch)*R(x,yaw)
    00384    *
    00385    * \see  "Robotik 1 Ausgabe Sommersemester 2001" by Prof. Dr. O. von Stryk
    00386    * \attention  RPY-angles are not clearly defined!
    00387    */
    00388   RotationMatrix& fromKardanRPY(const double yaw, const double pitch, const double roll);
    00389
    00390   /**
    00391    * Invert the matrix.
    00392    *
    00393    * \note: Inverted rotation matrix is transposed matrix.
    00394    */
    00395   RotationMatrix invert()
    00396   {
    00397     return transpose();
    00398   }
    00399
    00400   /**
    00401    * Rotation around the x-axis.
    00402    *
    00403    * \param   angle  The angle this pose will be rotated by
    00404    * \return  A reference to this object after the calculation.
    00405    */
    00406   RotationMatrix& rotateX(const double angle);
    00407
    00408   /**
    00409    * Rotation around the y-axis.
    00410    *
    00411    * \param   angle  The angle this pose will be rotated by
    00412    * \return  A reference to this object after the calculation.
    00413    */
    00414   RotationMatrix& rotateY(const double angle);
    00415
    00416   /**
    00417    * Rotation around the z-axis.
    00418    *
    00419    * \param   angle  The angle this pose will be rotated by
    00420    * \return  A reference to this object after the calculation.
    00421    */
    00422   RotationMatrix& rotateZ(const double angle);
    00423
    00424   /**
    00425    * Get the x-angle of a RotationMatrix.
    00426    *
    00427    * \return  The angle around the x-axis between the original
    00428    *          and the rotated z-axis projected on the y-z-plane
    00429    */
    00430   double getXAngle() const;
    00431
    00432   /**
    00433    * Get the y-angle of a RotationMatrix.
    00434    *
    00435    * \return  The angle around the y-axis between the original
    00436    *          and the rotated x-axis projected on the x-z-plane
    00437    */
    00438   double getYAngle() const;
    00439
    00440   /**
    00441    * Get the z-angle of a RotationMatrix.
    00442    *
    00443    * \return  The angle around the z-axis between the original
    00444    *          and the rotated x-axis projected on the x-y-plane
    00445    */
    00446   double getZAngle() const;
    00447
    00448   /**
    00449    * Create and return a RotationMatrix, rotated around x-axis
    00450    *
    00451    * \param   angle
    00452    * \return  rotated RotationMatrix
    00453    */
    00454   static RotationMatrix getRotationX(const double angle)
    00455   {
    00456     return RotationMatrix().rotateX(angle);
    00457   }
    00458
    00459   /**
    00460    * Create and return a RotationMatrix, rotated around y-axis
    00461    *
    00462    * \param   angle
    00463    * \return  rotated RotationMatrix
    00464    */
    00465   static RotationMatrix getRotationY(const double angle)
    00466   {
    00467     return RotationMatrix().rotateY(angle);
    00468   }
    00469
    00470   /**
    00471    * Create and return a RotationMatrix, rotated around z-axis
    00472    *
    00473    * \param   angle
    00474    * \return  rotated RotationMatrix
    00475    */
    00476   static RotationMatrix getRotationZ(const double angle)
    00477   {
    00478     return RotationMatrix().rotateZ(angle);
    00479   }
    00480 };
    00481
    00482 /**
    00483  * Streaming operator that reads a RotationMatrix from a stream.
    00484  *
    00485  * \param  stream          The stream from which is read.
    00486  * \param  rotationMatrix  The RotationMatrix object.
    00487  * \return The stream.
    00488  */
    00489 In& operator>>(In& stream, RotationMatrix& rotationMatrix);
    00490
    00491 /**
    00492  * Streaming operator that writes a RotationMatrix to a stream.
    00493  *
    00494  * \param  stream          The stream to write on.
    00495  * \param  rotationMatrix  The RotationMatrix object.
    00496  * \return The stream.
    00497  */
    00498 Out& operator<<(Out& stream, const RotationMatrix& rotationMatrix);
    00499
    00500
    00501
    00502 #endif // __Matrix_h__