Tools/Math/Pose2D.h

Go to the documentation of this file.

/**
 * @file Pose2D.h
 * Contains class Pose2D
 *
 * @author <a href="mailto:martin.kallnik@gmx.de">Martin Kallnik</a>
 * @author Max Risler
 */

#ifndef __Pose2D_h__
#define __Pose2D_h__

#include "Common.h"
#include "Vector2.h"
#include "Tools/Range.h"

/* choose the implementation of Pose2D*/
#define Pose2Dold // with angle for direction
//#define Pose2Dnew // with RotationVector for direction

#ifdef Pose2Dold

/** representation for 2D Transformation and Position (Location + Orientation)*/
class Pose2D {
  public:

  /** Rotation as an angle*/
  double rotation;

  /** translation as an vector2*/
  Vector2<double> translation;

  /** noargs-constructor*/
  Pose2D():rotation(0),translation(0,0) {}

  /** constructor from rotation and translation
   * \param rot rotation (double)
   * \param trans translation (Vector2)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const double rot, const Vector2<double>& trans):rotation(rot),translation(trans) {}

  /** constructor from rotation and translation
   * \param rot rotation (double)
   * \param x translation.x (double)
   * \param y translation.y (double)
   */
  Pose2D(const double rot, const double x, const double y):rotation(rot),translation(x,y)
  {}

  /** constructor from rotation
   * \param rot rotation (double)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const double rot):rotation(rot),translation(0,0) {}
  
  /** constructor from translation
   * \param trans translation (Vector2)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const Vector2<double>& trans):rotation(0),translation(trans) {}

  /** constructor from translation
   * \param trans translation (Vector2)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const Vector2<int>& trans):rotation(0),translation(trans.x,trans.y) {}

  /** constructor from two translation values
   * \param x translation x component
   * \param y translation y component
   */
  Pose2D(const double x, const double y):rotation(0),translation(x,y) {}

  /** get the Angle
  * @return Angle the Angle which defines the rotation
  */
  inline double getAngle() const {return rotation;}


  /** set rotation from Angle
  * @return the new Pose2D
  */
  inline Pose2D fromAngle(const double a) {rotation=a; return *this;}

  /** get the cos of the angle
  * @return the cos of the angle
  */
  inline double getCos() const {return cos(rotation);}

 /** get the sin of the angle
  * @return the sin of the angle
  */
  inline double getSin() const {return sin(rotation);}

  /** Assignment operator
  *\param other The other Pose2D that is assigned to this one
  *\return A reference to this object after the assignment.
  */
  Pose2D& operator=(const Pose2D& other)
  {
    rotation=other.rotation;
    translation=other.translation;
    return *this;
  }

  /** Copy constructor
  *\param other The other vector that is copied to this one
  */
  Pose2D(const Pose2D& other) {*this = other;}
  
  /** Multiplication of a Vector2 with this Pose2D
  *\param point The Vector2 that will be multiplicated with this Pose2D
  *\return The resulting Vector2
  */

  Vector2<double> operator*(const Vector2<double>& point) const
  { 
    double s=sin(rotation);
    double c=cos(rotation);
    return (Vector2<double>(point.x*c-point.y*s , point.x*s+point.y*c) + translation);
  }

  /** Comparison of another pose with this one.
  *\param other The other pose that will be compared to this one
  *\return Whether the two poses are equal.
  */
  bool operator==(const Pose2D& other) const
  {
    return ((translation==other.translation)&&(rotation==other.rotation));
  }

  /** Comparison of another pose with this one.
  *\param other The other pose that will be compared to this one
  *\return Whether the two poses are unequal.
  */
  bool operator!=(const Pose2D& other) const
    {return !(*this == other);}

  /**Concatenation of this pose with another pose.
  *\param other The other pose that will be concatenated to this one.
  *\return A reference to this pose after concatenation.
  */
  Pose2D& operator+=(const Pose2D& other)
  {
    translation = *this * other.translation;
    rotation += other.rotation;
    rotation = normalize(rotation);
    return *this;
  }

  /**A concatenation of this pose and another pose.
  *\param other The other pose that will be concatenated to this one.
  *\return The resulting pose.
  */
  Pose2D operator+(const Pose2D& other) const 
    {return Pose2D(*this) += other;}

  /**Subtracts a difference pose from this one to get the source pose. So if A+B=C is the addition/concatenation, this calculates C-B=A.
  *\param diff The difference Pose2D that shall be subtracted.
  *\return The resulting source pose. Adding diff to it again would give the this pose back.
  */
  Pose2D minusDiff(const Pose2D& diff) const
  {
    double rot=rotation-diff.rotation;
    double s=sin(rot);
    double c=cos(rot);
    return Pose2D(
      rot,
      translation.x - c*diff.translation.x + s*diff.translation.y,
      translation.y - c*diff.translation.y - s*diff.translation.x);
  }

  /**Difference of this pose relative to another pose. So if A+B=C is the addition/concatenation, this calculates C-A=B.
  *\param other The other pose that will be used as origin for the new pose.
  *\return A reference to this pose after calculating the difference.
  */
  Pose2D& operator-=(const Pose2D& other)
  {
    translation -= other.translation;
    Pose2D p(-other.rotation);
    return *this = p + *this;
  }

  /**Difference of this pose relative to another pose.
  *\param other The other pose that will be used as origin for the new pose.
  *\return The resulting pose.
  */
  Pose2D operator-(const Pose2D& other) const 
    {return Pose2D(*this) -= other;}

  /**Concatenation of this pose with another pose
  *\param other The other pose that will be concatenated to this one.
  *\return A reference to this pose after concatenation
  */
  Pose2D& conc(const Pose2D& other)
    {return *this += other;}

  /**Translate this pose by a translation vector
  *\param trans Vector to translate with
  *\return A reference to this pose after translation
  */
  Pose2D& translate(const Vector2<double>& trans)
  {
    translation = *this * trans;
    return *this;
  }

  /**Translate this pose by a translation vector
  *\param x x component of vector to translate with
  *\param y y component of vector to translate with
  *\return A reference to this pose after translation
  */
  Pose2D& translate(const double x, const double y)
  {
    translation = *this * Vector2<double>(x,y);
    return *this;
  }


  /**Rotate this pose by a rotation
  *\param angle Angle to rotate.
  *\return A reference to this pose after rotation
  */
  Pose2D& rotate(const double angle)
  {
    rotation += angle;
    return *this;
  }

  /**
  * The function creates a random pose.
  * @param x The range for x-values of the pose.
  * @param y The range for y-values of the pose.
  * @param angle The range for the rotation of the pose.
  */
  static Pose2D random(const Range<double>& x,
                       const Range<double>& y,
                       const Range<double>& angle)
  { // angle should even work in wrap around case!  
    return Pose2D(::random() * (angle.max - angle.min) + angle.min,
                  Vector2<double>(::random() * (x.max - x.min) + x.min,
                                  ::random() * (y.max - y.min) + y.min));
  }
};

#endif //Pose2Dold

#ifdef Pose2Dnew

/** @class Represents an Rotation in a plane (2D) by the sinus and
 * cosinus of the angle.  It is the half of an 3x3 RotationMatrix of
 * an rotation around the z-axis.  the RotationMatrix an rotation
 * around the z-axis contains 4 zeros, one 1, twice the cosinus of the
 * angle and the sinus and negativ sinus of the rotationangle. This
 * vector just stores the sinus and cosinus to save memory.  Storing
 * just the angle is also possible, but if you store sinus and cosinus
 * you do not need to calculate sinus and cosinus each time you
 * concatenate two Pose2D.
 * @todo is it better to save the angle also?
 */
class RotationVector {
 public:
  /** the sinus of the angle*/
  double sinus;
  
  /** the cosinus of the angle*/
  double cosinus;

  /** no-args constructor*/
  RotationVector() {sinus=0.0; cosinus=1.0;}

  /** constructor from Angle
   * @param a the angle which defines the rotation
   */
  RotationVector(const double a) {sinus = sin(a); cosinus = cos(a);};

  /** constructor from sinus and cosinus
   * The constructor normalizes both values if required.
   * @param c the cosinus of the angle
   * @param s the sinus of the angle
   */
  RotationVector(double c, double s) {
    double l = s * s + c * c;
    if(abs(l - 1) > 0.000001)
    {
      l = sqrt(l);
      s /= l;
      c /= l;
    }
    sinus = s; cosinus=c;
  }
  
  /** assignment operator
   * @param other The other matrix that is assigned to this one
   * @return A reference to this object after the assignment.
   */
  RotationVector& operator=(const RotationVector& other) {
    this->sinus = other.sinus;
    this->cosinus = other.cosinus;
    return *this;
  }
  
  /** copyconstructor
   * @param r the Rotationvector which is copied
   */
  RotationVector(const RotationVector& r) {*this = r;};

  /** Invert the Rotation
   * @return the inverted Rotation
   */
  RotationVector invert() {
    this->sinus = -this->sinus;
    return *this;
  }

  /** Rotation from Angle
   * @param a the Angle
   * @return the Rotation defined by the Angle
   */
  RotationVector fromAngle(const double a) {
    sinus = sin(a);
    cosinus = cos(a);
    return *this;
  }


  /** get the Angle
   * @return Angle the Angle which defines the rotation
   */
  double getAngle() const {return atan2(sinus,cosinus);}
  
  
  /** Rotation from Angle in  Degrees
   * @param d the Angle
   * @return the Rotation defined by the Angle
   */
  RotationVector fromDegrees(const double d) {
    double a = ::fromDegrees(d);
    sinus = sin(a);
    cosinus = cos(a);
    return *this;
  }
      
  /** get the Angle in Degrees
   * @return Angle the Angle which defines the rotation
   */
  double toDegrees() const {return ::toDegrees(atan2(sinus,cosinus));}
  
  /** rotate a translationvector
   * @param v the translation
   * @return the rotated translation*/
  Vector2<double> operator*(const Vector2<double>& v) const {
    return Vector2<double>(cosinus * v.x - sinus * v.y,sinus * v.x + cosinus * v.y);
  }

  /** concatenate to rotations
   * @param other the other rotation
   * @return the concatenation
   */
  RotationVector operator*(const RotationVector& other) const {
    return RotationVector(cosinus * other.cosinus - sinus * other.sinus,
                          sinus * other.cosinus + cosinus * other.sinus);
  }

  /** concatenate to rotations
   * @param other the other rotation
   * @return the concatenation
   */
  RotationVector operator+(const RotationVector& other) const {
    return RotationVector(cosinus * other.cosinus - sinus * other.sinus,
                          sinus * other.cosinus + cosinus * other.sinus);
  }

  /** concatenate to rotations (invers of second rotation)
   * @param other the other rotation
   * @return the concatenation
   */
  RotationVector operator-(const RotationVector& other) const {
    return RotationVector(cosinus * other.cosinus + sinus * other.sinus,
                          sinus * other.cosinus - cosinus * other.sinus);
  }

  /** concatenate to rotations (this= this*other)
   * @param other the other rotation
   * @return the concatenation
   */
  RotationVector operator*=(const RotationVector& other) {
    return *this = RotationVector(cosinus * other.cosinus - sinus * other.sinus,
                                  sinus * other.cosinus + cosinus * other.sinus);
  }

 /** concatenate to rotations (this= this*other)
   * @param other the other rotation
   * @return the concatenation
   */
  RotationVector operator+=(const RotationVector& other) {
    return *this = RotationVector(cosinus * other.cosinus - sinus * other.sinus,
                                  sinus * other.cosinus + cosinus * other.sinus);
  }

 /** concatenate to rotations (this= this*other)(second is inverted)
   * @param other the other rotation
   * @return the concatenation
   */
  RotationVector operator-=(const RotationVector& other) {
    return *this = RotationVector(cosinus * other.cosinus + sinus * other.sinus,
                                  sinus * other.cosinus - cosinus * other.sinus);
  }


  /** Comparison of two RotationVectors
   * @param other the other RotationVector
   * @return bool wether the RotationVectors are equal
   */
  bool operator==(const RotationVector& other) const {
    return sinus == other.sinus && cosinus == other.cosinus;
  }

  /** Comparison of two RotationVectors
   * @param other the other RotationVector
   * @return bool wether the RotationVectors are unequal
   */
  bool operator!=(const RotationVector& other) const {
    return sinus != other.sinus || cosinus != other.cosinus;
  }

  /** unary addition*/
  RotationVector operator+() const {return *this;}
  
  /** unary subtraction*/
  RotationVector operator-() const {return RotationVector(cosinus,-sinus);}
};

/**
* Streaming operator that reads a RotationVector from a stream.
* @param stream The stream from which is read.
* @param rotationVector The RotationVector object.
* @return The stream.
*/ 
In& operator>>(In& stream, RotationVector& rotationVector);

/**
* Streaming operator that writes a RotationVector to a stream.
* @param stream The stream to write on.
* @param rotationVector The RotationVector object.
* @return The stream.
*/ 
Out& operator<<(Out& stream, const RotationVector& rotationVector);

/**
 *
 */
class Pose2D {
  public:
  /** the rotation*/
  RotationVector rotation;

  /** the translation*/
  Vector2<double> translation;

  /** noargs-constructor*/
  Pose2D(){};

  /** constructor from rotation and translation
   * @param rot rotation (Angle)
   * @param trans translation (Vector2)
   */
  /* vc++ bug, translation = trans is workaround */
//    Pose2D(const Angle& rot, const Vector2<double>& trans) {rotation = rot; translation=trans;}

  /** constructor from rotation and translation
   * @param rot rotation (RotationVector)
   * @param trans translation (Vector2)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const RotationVector& rot, const Vector2<double>& trans) {rotation = rot; translation=trans;}

  /** constructor from rotation and translation
   * @param rot rotation (Angle)
   * @param x translation.x (double)
   * @param y translation.y (double)
   */
/*   Pose2D(const Angle& rot, const double x, const double y) {
    rotation = rot; translation.x = x, translation.y = y; 
  }*/

  /** constructor from rotation and translation
   * @param rot rotation (RotationVector)
   * @param x translation.x (double)
   * @param y translation.y (double)
   */
  Pose2D(const RotationVector& rot, const double x, const double y) {
    rotation = rot; translation.x = x, translation.y = y; 
  }
  
  /** constructor from rotation
   * @param rot rotation (Angle)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const double rot) {rotation = rot;}

  /** constructor from rotation
   * @param rot rotation (RotationVector)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const RotationVector& rot) {rotation = rot;}
    
  /** constructor from translation
   * @param trans translation (Vector2)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const Vector2<double>& trans) {translation = trans;}

  /** constructor from translation
   * @param trans translation (Vector2)
   */
  /* vc++ bug, translation = trans is workaround */
  Pose2D(const Vector2<int>& trans) {translation.x = trans.x; translation.y = trans.y;}


  /** constructor from two translation values
   * @param x translation x component
   * @param y translation y component
   */
  Pose2D(const double x, const double y) {translation = Vector2<double>(x,y); }

  /** Assignment operator
   * @param other The other Pose2D that is assigned to this one
   * @return A reference to this object after the assignment.
   */
  Pose2D& operator=(const Pose2D& other){
    rotation = other.rotation;
    translation = other.translation;
    return *this;
  }

  /** Copy constructor
   * @param other The other vector that is copied to this one
   */
  Pose2D(const Pose2D& other) {*this = other;}
  
  /** get the Angle
  * @return Angle the Angle which defines the rotation
  */
  double getAngle() const {return rotation.getAngle();}

  /** set the Rotation from an angle
  * @return the pose2D
  */
  Pose2D fromAngle(const double a) {rotation.fromAngle(a); return *this;}

  /** get the Sinus of the Angle
  * @return Sinus of the Angle the Angle which defines the rotation
  */
  double getSin() const {return rotation.sinus;}

  /** get the Cosinus of the Angle
  * @return Cosinus of the Angle the Angle which defines the rotation
  */
  double getCos() const {return rotation.sinus;}

  
  /** Multiplication of a Vector2 with this Pose2D
   * @param point The Vector2 that will be multiplicated with this Pose2D
   * @return The resulting Vector2
   */
  Vector2<double> operator*(const Vector2<double>& point) const {
    return translation + rotation * point;
  }

  /** Comparison of another pose with this one.
   * @param other The other pose that will be compared to this one
   * @return Whether the two poses are equal.
   */
  bool operator==(const Pose2D& other) const {
    return translation == other.translation && rotation == other.rotation;
  }

  /** Comparison of another pose with this one.
   * @param other The other pose that will be compared to this one
   * @return Whether the two poses are unequal.
   */
  bool operator!=(const Pose2D& other) const {return !(*this == other);}

  /**Concatenation of this pose with another pose.
   * @param other The other pose that will be concatenated to this one.
   * @return A reference to this pose after concatenation.
   */
  Pose2D& operator+=(const Pose2D& other) {
    translation = *this * other.translation;
    rotation *= other.rotation;
    return *this;
  }

  /**A concatenation of this pose and another pose.
   * @param other The other pose that will be concatenated to this one.
   * @return The resulting pose.
   */
  Pose2D operator+(const Pose2D& other) const {
    return Pose2D(*this) += other;
  }
  
  /**Difference of this pose relative to another pose.
   * @param other The other pose that will be used as origin for the new pose.
   * @return A reference to this pose after calculating the difference.
   */
  Pose2D& operator-=(const Pose2D& other) {
    translation -= other.translation;
    Pose2D p(-other.rotation);
    return *this = p + *this;
  }

  /**Difference of this pose relative to another pose.
   * @param other The other pose that will be used as origin for the new pose.
   * @return The resulting pose.
   */
  Pose2D operator-(const Pose2D& other) const {
    return Pose2D(*this) -= other;
  }

  /**Concatenation of this pose with another pose
   * @param other The other pose that will be concatenated to this one.
   * @return A reference to this pose after concatenation
   */
  Pose2D& conc(const Pose2D& other) {
    return *this += other;
  }

  /**Translate this pose by a translation vector
   * @param trans Vector to translate with
   * @return A reference to this pose after translation
   */
  Pose2D& translate(const Vector2<double>& trans) {
    translation = *this * trans;
    return *this;
  }

  /**Translate this pose by a translation vector
   * @param x x component of vector to translate with
   * @param y y component of vector to translate with
   * @return A reference to this pose after translation
   */
  Pose2D& translate(const double x, const double y) {
    translation = *this * Vector2<double>(x,y);
    return *this;
  }

  /**Rotate this pose by a rotation
   * @param rot Angle to rotate with
   * @return A reference to this pose after rotation
   */
  Pose2D& rotate(const double angle) {
    rotation *= RotationVector(angle);
    return *this;
  } 

  /**Rotate this pose by a rotation
   * @param rot RotationVector to rotate with
   * @return A reference to this pose after rotation
   */
  Pose2D& rotate(const RotationVector& rot) {
    rotation *= rot;
    return *this;
  } 

  /**
  * The function creates a random pose.
  * @param x The range for x-values of the pose.
  * @param y The range for y-values of the pose.
  * @param angle The range for the rotation of the pose.
  */
  static Pose2D random(const Range<double>& x,
                       const Range<double>& y,
                       const Range<double>& angle)
  { // angle should even work in wrap around case!  
    return Pose2D(::random() * (angle.max - angle.min) + angle.min,
                  Vector2<double>(::random() * (x.max - x.min) + x.min,
                                  ::random() * (y.max - y.min) + y.min));
  }
};

#endif //Pose2Dnew

/**
* Streaming operator that reads a Pose2D from a stream.
* @param stream The stream from which is read.
* @param pose2D The Pose2D object.
* @return The stream.
*/ 
In& operator>>(In& stream, Pose2D& pose2D);

/**
* Streaming operator that writes a Pose2D to a stream.
* @param stream The stream to write on.
* @param pose2D The Pose2D object.
* @return The stream.
*/ 
Out& operator<<(Out& stream, const Pose2D& pose2D);


/* *Concatenation of two 2D poses
 * \param p1 pose1
 * \param p2 pose2
 * /
static Pose2D conc(const Pose2D& p1, const Pose2D& p2){
  return Pose2D(p1.rotation + p2.rotation, p1 * p2.translation);
}
*/

#endif // __Pose2D_h__

---