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

# Tools/Math/Matrix.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file Matrix.cpp
    00003  * Implements RotationMatrix
    00004  *
    00005  * @author <a href="mailto:martin.kallnik@gmx.de" > Martin Kallnik</a>
    00006  * @author Max Risler
    00007  */
    00008
    00009 #include "Matrix.h"
    00010 #include "Common.h"
    00011 #include "Tools/Streams/Streamable.h"
    00012
    00013 RotationMatrix& RotationMatrix::fromKardanRPY
    00014 (const double yaw, const double pitch, const double roll){
    00015
    00016   double cy=cos(yaw);
    00017   double sy=sin(yaw);
    00018   double cp=cos(pitch);
    00019   double sp=sin(pitch);
    00020   double cr=cos(roll);
    00021   double sr=sin(roll);
    00022
    00023   c[0].x=cr*cp ;
    00024   c[0].y=-sr*cy+cr*sp*sy ;
    00025   c[0].z=sr*sy+cr*sp*cy ;
    00026   c[1].x=sr*cp ;
    00027   c[1].y=cr*cy+sr*sp*sy ;
    00028   c[1].z=-cr*sy+sr*sp*cy ;
    00029   c[2].x=-sp ;
    00030   c[2].y=cp*sy ;
    00031   c[2].z=cp*cy ;
    00032
    00033   return *this;
    00034 }
    00035
    00036 RotationMatrix& RotationMatrix::rotateX(const double angle)
    00037 {
    00038   double c = cos(angle),
    00039          s = sin(angle);
    00040   *this *= RotationMatrix(Vector3<double>(1,0,0),
    00041                           Vector3<double>(0,c,s),
    00042                           Vector3<double>(0,-s,c));
    00043   return *this;
    00044 }
    00045
    00046 RotationMatrix& RotationMatrix::rotateY(const double angle)
    00047 {
    00048   double c = cos(angle),
    00049          s = sin(angle);
    00050   *this *= RotationMatrix(Vector3<double>(c,0,-s),
    00051                           Vector3<double>(0,1,0),
    00052                           Vector3<double>(s,0,c));
    00053   return *this;
    00054 }
    00055
    00056 RotationMatrix& RotationMatrix::rotateZ(const double angle)
    00057 {
    00058   double c = cos(angle),
    00059          s = sin(angle);
    00060   *this *= RotationMatrix(Vector3<double>(c,s,0),
    00061                           Vector3<double>(-s,c,0),
    00062                           Vector3<double>(0,0,1));
    00063   return *this;
    00064 }
    00065
    00066 double RotationMatrix::getXAngle() const
    00067 {
    00068   double h = sqrt(c[2].y * c[2].y + c[2].z * c[2].z);
    00069   return h ? acos(c[2].z / h) * (c[2].y > 0 ? -1 : 1) : 0;
    00070 }
    00071
    00072 double RotationMatrix::getYAngle() const
    00073 {
    00074   double h = sqrt(c[0].x * c[0].x + c[0].z * c[0].z);
    00075   return h ? acos(c[0].x / h) * (c[0].z > 0 ? -1 : 1) : 0;
    00076 }
    00077
    00078 double RotationMatrix::getZAngle() const
    00079 {
    00080   double h = sqrt(c[0].x * c[0].x + c[0].y * c[0].y);
    00081   return h ? acos(c[0].x / h) * (c[0].y < 0 ? -1 : 1) : 0;
    00082 }
    00083
    00084 template <class V> In& operator>>(In& stream, Matrix3x3<V>& matrix3x3)
    00085 {
    00086   STREAM_REGISTER_BEGIN_EXT( matrix3x3);
    00087   STREAM_EXT( stream, matrix3x3.c[0]);
    00088   STREAM_EXT( stream, matrix3x3.c[1]);
    00089   STREAM_EXT( stream, matrix3x3.c[2]);
    00090   STREAM_REGISTER_FINISH();
    00091
    00092   /*
    00093   stream >> matrix3x3.c[0];
    00094   stream >> matrix3x3.c[1];
    00095   stream >> matrix3x3.c[2];
    00096   */
    00097   return stream;
    00098 }
    00099
    00100 template <class V> Out& operator<<(Out& stream, const Matrix3x3<V>& matrix3x3)
    00101 {
    00102   STREAM_REGISTER_BEGIN_EXT( matrix3x3);
    00103   STREAM_EXT( stream, matrix3x3.c[0]);
    00104   STREAM_EXT( stream, matrix3x3.c[1]);
    00105   STREAM_EXT( stream, matrix3x3.c[2]);
    00106   STREAM_REGISTER_FINISH();
    00107   /*
    00108   stream << matrix3x3.c[0];
    00109   stream << matrix3x3.c[1];
    00110   stream << matrix3x3.c[2];
    00111   */
    00112   return stream;
    00113 }
    00114
    00115 In& operator>>(In& stream, RotationMatrix& rotationMatrix)
    00116 {
    00117   STREAM_REGISTER_BEGIN_EXT( rotationMatrix);
    00118   STREAM_EXT( stream, rotationMatrix.c[0]);
    00119   STREAM_EXT( stream, rotationMatrix.c[1]);
    00120   STREAM_EXT( stream, rotationMatrix.c[2]);
    00121   STREAM_REGISTER_FINISH();
    00122   /*
    00123   stream >> rotationMatrix.c[0];
    00124   stream >> rotationMatrix.c[1];
    00125   stream >> rotationMatrix.c[2];
    00126   */
    00127   return stream;
    00128 }
    00129
    00130 Out& operator<<(Out& stream, const RotationMatrix& rotationMatrix)
    00131 {
    00132   STREAM_REGISTER_BEGIN_EXT( rotationMatrix);
    00133   STREAM_EXT( stream, rotationMatrix.c[0]);
    00134   STREAM_EXT( stream, rotationMatrix.c[1]);
    00135   STREAM_EXT( stream, rotationMatrix.c[2]);
    00136   STREAM_REGISTER_FINISH();
    00137   /*
    00138   stream << rotationMatrix.c[0];
    00139   stream << rotationMatrix.c[1];
    00140   stream << rotationMatrix.c[2];
    00141   */
    00142   return stream;
    00143 }