Tools/Math/Matrix.cpp

Go to the documentation of this file.

/**
 * @file Matrix.cpp
 * Implements RotationMatrix
 *
 * @author <a href="mailto:martin.kallnik@gmx.de" > Martin Kallnik</a>
 * @author Max Risler
 */

#include "Matrix.h"
#include "Common.h"

RotationMatrix& RotationMatrix::fromKardanRPY
(const double yaw, const double pitch, const double roll){

  double cy=cos(yaw);
  double sy=sin(yaw);
  double cp=cos(pitch);
  double sp=sin(pitch);
  double cr=cos(roll);
  double sr=sin(roll);

  c[0].x=cr*cp ;
  c[0].y=-sr*cy+cr*sp*sy ;
  c[0].z=sr*sy+cr*sp*cy ;
  c[1].x=sr*cp ;
  c[1].y=cr*cy+sr*sp*sy ;
  c[1].z=-cr*sy+sr*sp*cy ;
  c[2].x=-sp ;
  c[2].y=cp*sy ;
  c[2].z=cp*cy ;

  return *this;
}
      
RotationMatrix& RotationMatrix::rotateX(const double angle)
{
  double c = cos(angle),
         s = sin(angle);
  *this *= RotationMatrix(Vector3<double>(1,0,0),
                          Vector3<double>(0,c,s),
                          Vector3<double>(0,-s,c));
  return *this;
}

RotationMatrix& RotationMatrix::rotateY(const double angle)
{
  double c = cos(angle),
         s = sin(angle);
  *this *= RotationMatrix(Vector3<double>(c,0,-s),
                          Vector3<double>(0,1,0),
                          Vector3<double>(s,0,c));
  return *this;
}

RotationMatrix& RotationMatrix::rotateZ(const double angle)
{
  double c = cos(angle),
         s = sin(angle);
  *this *= RotationMatrix(Vector3<double>(c,s,0),
                          Vector3<double>(-s,c,0),
                          Vector3<double>(0,0,1));
  return *this;
}

double RotationMatrix::getXAngle() const
{
  double h = sqrt(c[2].y * c[2].y + c[2].z * c[2].z);
  return h ? acos(c[2].z / h) * (c[2].y > 0 ? -1 : 1) : 0;
}

double RotationMatrix::getYAngle() const
{
  double h = sqrt(c[0].x * c[0].x + c[0].z * c[0].z);
  return h ? acos(c[0].x / h) * (c[0].z > 0 ? -1 : 1) : 0;
}

double RotationMatrix::getZAngle() const
{
  double h = sqrt(c[0].x * c[0].x + c[0].y * c[0].y);
  return h ? acos(c[0].x / h) * (c[0].y < 0 ? -1 : 1) : 0;
}

template <class V> In& operator>>(In& stream, Matrix3x3<V>& matrix3x3)
{
  stream >> matrix3x3.c[0];
  stream >> matrix3x3.c[1];
  stream >> matrix3x3.c[2];
  return stream;
}

template <class V> Out& operator<<(Out& stream, const Matrix3x3<V>& matrix3x3)
{
  stream << matrix3x3.c[0];
  stream << matrix3x3.c[1];
  stream << matrix3x3.c[2];
  return stream;
}

In& operator>>(In& stream, RotationMatrix& rotationMatrix)
{
  stream >> rotationMatrix.c[0];
  stream >> rotationMatrix.c[1];
  stream >> rotationMatrix.c[2];
  return stream;
}

Out& operator<<(Out& stream, const RotationMatrix& rotationMatrix)
{
  stream << rotationMatrix.c[0];
  stream << rotationMatrix.c[1];
  stream << rotationMatrix.c[2];
  return stream;
}


/*
* Change log :
* 
* $Log: Matrix.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:37:11  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2003/12/02 13:44:55  cesarz
* added streaming operators
*
* Revision 1.1  2003/10/07 10:13:24  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.4  2002/11/19 15:43:04  dueffert
* doxygen comments corrected
*
* Revision 1.3  2002/11/12 23:00:47  dueffert
* started restore greenhills compatibility
*
* Revision 1.2  2002/09/22 18:40:52  risler
* added new math functions, removed GTMath library
*
* Revision 1.1  2002/09/22 13:10:50  risler
* new Math headers added
*
*
*/

---