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

# Tools/Math/Pose2D.h

Go to the documentation of this file.

    00001 /**
    00002  * @file Pose2D.h
    00003  * Contains class Pose2D
    00004  *
    00005  * @author <a href="mailto:martin.kallnik@gmx.de">Martin Kallnik</a>
    00006  * @author Max Risler
    00007  */
    00008
    00009 #ifndef __Pose2D_h__
    00010 #define __Pose2D_h__
    00011
    00012 #include "Common.h"
    00013 #include "Vector2.h"
    00014 #include "Tools/Range.h"
    00015
    00016 /* choose the implementation of Pose2D*/
    00017 #define Pose2Dold // with angle for direction
    00018 //#define Pose2Dnew // with RotationVector for direction
    00019
    00020 #ifdef Pose2Dold
    00021
    00022 /** representation for 2D Transformation and Position (Location + Orientation)*/
    00023 class Pose2D {
    00024   public:
    00025
    00026   /** Rotation as an angle*/
    00027   double rotation;
    00028
    00029   /** translation as an vector2*/
    00030   Vector2<double> translation;
    00031
    00032   /** noargs-constructor*/
    00033   Pose2D():rotation(0),translation(0,0) {}
    00034
    00035   /** constructor from rotation and translation
    00036    * \param rot rotation (double)
    00037    * \param trans translation (Vector2)
    00038    */
    00039   /* vc++ bug, translation = trans is workaround */
    00040   Pose2D(const double rot, const Vector2<double>& trans):rotation(rot),translation(trans) {}
    00041
    00042   /** constructor from rotation and translation
    00043    * \param rot rotation (double)
    00044    * \param x translation.x (double)
    00045    * \param y translation.y (double)
    00046    */
    00047   Pose2D(const double rot, const double x, const double y):rotation(rot),translation(x,y)
    00048   {}
    00049
    00050   /** constructor from rotation
    00051    * \param rot rotation (double)
    00052    */
    00053   /* vc++ bug, translation = trans is workaround */
    00054   Pose2D(const double rot):rotation(rot),translation(0,0) {}
    00055
    00056   /** constructor from translation
    00057    * \param trans translation (Vector2)
    00058    */
    00059   /* vc++ bug, translation = trans is workaround */
    00060   Pose2D(const Vector2<double>& trans):rotation(0),translation(trans) {}
    00061
    00062   /** constructor from translation
    00063    * \param trans translation (Vector2)
    00064    */
    00065   /* vc++ bug, translation = trans is workaround */
    00066   Pose2D(const Vector2<int>& trans):rotation(0),translation(trans.x,trans.y) {}
    00067
    00068   /** constructor from two translation values
    00069    * \param x translation x component
    00070    * \param y translation y component
    00071    */
    00072   Pose2D(const double x, const double y):rotation(0),translation(x,y) {}
    00073
    00074   /** get the Angle
    00075   * @return Angle the Angle which defines the rotation
    00076   */
    00077   inline double getAngle() const {return rotation;}
    00078
    00079
    00080   /** set rotation from Angle
    00081   * @return the new Pose2D
    00082   */
    00083   inline Pose2D fromAngle(const double a) {rotation=a; return *this;}
    00084
    00085   /** get the cos of the angle
    00086   * @return the cos of the angle
    00087   */
    00088   inline double getCos() const {return cos(rotation);}
    00089
    00090  /** get the sin of the angle
    00091   * @return the sin of the angle
    00092   */
    00093   inline double getSin() const {return sin(rotation);}
    00094
    00095   /** Assignment operator
    00096   *\param other The other Pose2D that is assigned to this one
    00097   *\return A reference to this object after the assignment.
    00098   */
    00099   Pose2D& operator=(const Pose2D& other)
    00100   {
    00101     rotation=other.rotation;
    00102     translation=other.translation;
    00103     return *this;
    00104   }
    00105
    00106   /** Copy constructor
    00107   *\param other The other vector that is copied to this one
    00108   */
    00109   Pose2D(const Pose2D& other) {*this = other;}
    00110
    00111   /** Multiplication of a Vector2 with this Pose2D
    00112   *\param point The Vector2 that will be multiplicated with this Pose2D
    00113   *\return The resulting Vector2
    00114   */
    00115
    00116   Vector2<double> operator*(const Vector2<double>& point) const
    00117   {
    00118     double s=sin(rotation);
    00119     double c=cos(rotation);
    00120     return (Vector2<double>(point.x*c-point.y*s , point.x*s+point.y*c) + translation);
    00121   }
    00122
    00123   /** Comparison of another pose with this one.
    00124   *\param other The other pose that will be compared to this one
    00125   *\return Whether the two poses are equal.
    00126   */
    00127   bool operator==(const Pose2D& other) const
    00128   {
    00129     return ((translation==other.translation)&&(rotation==other.rotation));
    00130   }
    00131
    00132   /** Comparison of another pose with this one.
    00133   *\param other The other pose that will be compared to this one
    00134   *\return Whether the two poses are unequal.
    00135   */
    00136   bool operator!=(const Pose2D& other) const
    00137     {return !(*this == other);}
    00138
    00139   /**Concatenation of this pose with another pose.
    00140   *\param other The other pose that will be concatenated to this one.
    00141   *\return A reference to this pose after concatenation.
    00142   */
    00143   Pose2D& operator+=(const Pose2D& other)
    00144   {
    00145     translation = *this * other.translation;
    00146     rotation += other.rotation;
    00147     rotation = normalize(rotation);
    00148     return *this;
    00149   }
    00150
    00151   /**A concatenation of this pose and another pose.
    00152   *\param other The other pose that will be concatenated to this one.
    00153   *\return The resulting pose.
    00154   */
    00155   Pose2D operator+(const Pose2D& other) const
    00156     {return Pose2D(*this) += other;}
    00157
    00158   /**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.
    00159   *\param diff The difference Pose2D that shall be subtracted.
    00160   *\return The resulting source pose. Adding diff to it again would give the this pose back.
    00161   */
    00162   Pose2D minusDiff(const Pose2D& diff) const
    00163   {
    00164     double rot=rotation-diff.rotation;
    00165     double s=sin(rot);
    00166     double c=cos(rot);
    00167     return Pose2D(
    00168       rot,
    00169       translation.x - c*diff.translation.x + s*diff.translation.y,
    00170       translation.y - c*diff.translation.y - s*diff.translation.x);
    00171   }
    00172
    00173   /**Difference of this pose relative to another pose. So if A+B=C is the addition/concatenation, this calculates C-A=B.
    00174   *\param other The other pose that will be used as origin for the new pose.
    00175   *\return A reference to this pose after calculating the difference.
    00176   */
    00177   Pose2D& operator-=(const Pose2D& other)
    00178   {
    00179     translation -= other.translation;
    00180     Pose2D p(-other.rotation);
    00181     return *this = p + *this;
    00182   }
    00183
    00184   /**Difference of this pose relative to another pose.
    00185   *\param other The other pose that will be used as origin for the new pose.
    00186   *\return The resulting pose.
    00187   */
    00188   Pose2D operator-(const Pose2D& other) const
    00189     {return Pose2D(*this) -= other;}
    00190
    00191   /**Concatenation of this pose with another pose
    00192   *\param other The other pose that will be concatenated to this one.
    00193   *\return A reference to this pose after concatenation
    00194   */
    00195   Pose2D& conc(const Pose2D& other)
    00196     {return *this += other;}
    00197
    00198   /**Translate this pose by a translation vector
    00199   *\param trans Vector to translate with
    00200   *\return A reference to this pose after translation
    00201   */
    00202   Pose2D& translate(const Vector2<double>& trans)
    00203   {
    00204     translation = *this * trans;
    00205     return *this;
    00206   }
    00207
    00208   /**Translate this pose by a translation vector
    00209   *\param x x component of vector to translate with
    00210   *\param y y component of vector to translate with
    00211   *\return A reference to this pose after translation
    00212   */
    00213   Pose2D& translate(const double x, const double y)
    00214   {
    00215     translation = *this * Vector2<double>(x,y);
    00216     return *this;
    00217   }
    00218
    00219
    00220   /**Rotate this pose by a rotation
    00221   *\param angle Angle to rotate.
    00222   *\return A reference to this pose after rotation
    00223   */
    00224   Pose2D& rotate(const double angle)
    00225   {
    00226     rotation += angle;
    00227     return *this;
    00228   }
    00229
    00230   /**
    00231   * The function creates a random pose.
    00232   * @param x The range for x-values of the pose.
    00233   * @param y The range for y-values of the pose.
    00234   * @param angle The range for the rotation of the pose.
    00235   */
    00236   static Pose2D random(const Range<double>& x,
    00237                        const Range<double>& y,
    00238                        const Range<double>& angle)
    00239   { // angle should even work in wrap around case!
    00240     return Pose2D(::random() * (angle.max - angle.min) + angle.min,
    00241                   Vector2<double>(::random() * (x.max - x.min) + x.min,
    00242                                   ::random() * (y.max - y.min) + y.min));
    00243   }
    00244 };
    00245
    00246 #endif //Pose2Dold
    00247
    00248 #ifdef Pose2Dnew
    00249
    00250 /** @class Represents an Rotation in a plane (2D) by the sinus and
    00251  * cosinus of the angle.  It is the half of an 3x3 RotationMatrix of
    00252  * an rotation around the z-axis.  the RotationMatrix an rotation
    00253  * around the z-axis contains 4 zeros, one 1, twice the cosinus of the
    00254  * angle and the sinus and negativ sinus of the rotationangle. This
    00255  * vector just stores the sinus and cosinus to save memory.  Storing
    00256  * just the angle is also possible, but if you store sinus and cosinus
    00257  * you do not need to calculate sinus and cosinus each time you
    00258  * concatenate two Pose2D.
    00259  * @todo is it better to save the angle also?
    00260  */
    00261 class RotationVector {
    00262  public:
    00263   /** the sinus of the angle*/
    00264   double sinus;
    00265
    00266   /** the cosinus of the angle*/
    00267   double cosinus;
    00268
    00269   /** no-args constructor*/
    00270   RotationVector() {sinus=0.0; cosinus=1.0;}
    00271
    00272   /** constructor from Angle
    00273    * @param a the angle which defines the rotation
    00274    */
    00275   RotationVector(const double a) {sinus = sin(a); cosinus = cos(a);};
    00276
    00277   /** constructor from sinus and cosinus
    00278    * The constructor normalizes both values if required.
    00279    * @param c the cosinus of the angle
    00280    * @param s the sinus of the angle
    00281    */
    00282   RotationVector(double c, double s) {
    00283     double l = s * s + c * c;
    00284     if(abs(l - 1) > 0.000001)
    00285     {
    00286       l = sqrt(l);
    00287       s /= l;
    00288       c /= l;
    00289     }
    00290     sinus = s; cosinus=c;
    00291   }
    00292
    00293   /** assignment operator
    00294    * @param other The other matrix that is assigned to this one
    00295    * @return A reference to this object after the assignment.
    00296    */
    00297   RotationVector& operator=(const RotationVector& other) {
    00298     this->sinus = other.sinus;
    00299     this->cosinus = other.cosinus;
    00300     return *this;
    00301   }
    00302
    00303   /** copyconstructor
    00304    * @param r the Rotationvector which is copied
    00305    */
    00306   RotationVector(const RotationVector& r) {*this = r;};
    00307
    00308   /** Invert the Rotation
    00309    * @return the inverted Rotation
    00310    */
    00311   RotationVector invert() {
    00312     this->sinus = -this->sinus;
    00313     return *this;
    00314   }
    00315
    00316   /** Rotation from Angle
    00317    * @param a the Angle
    00318    * @return the Rotation defined by the Angle
    00319    */
    00320   RotationVector fromAngle(const double a) {
    00321     sinus = sin(a);
    00322     cosinus = cos(a);
    00323     return *this;
    00324   }
    00325
    00326
    00327   /** get the Angle
    00328    * @return Angle the Angle which defines the rotation
    00329    */
    00330   double getAngle() const {return atan2(sinus,cosinus);}
    00331
    00332
    00333   /** Rotation from Angle in  Degrees
    00334    * @param d the Angle
    00335    * @return the Rotation defined by the Angle
    00336    */
    00337   RotationVector fromDegrees(const double d) {
    00338     double a = ::fromDegrees(d);
    00339     sinus = sin(a);
    00340     cosinus = cos(a);
    00341     return *this;
    00342   }
    00343
    00344   /** get the Angle in Degrees
    00345    * @return Angle the Angle which defines the rotation
    00346    */
    00347   double toDegrees() const {return ::toDegrees(atan2(sinus,cosinus));}
    00348
    00349   /** rotate a translationvector
    00350    * @param v the translation
    00351    * @return the rotated translation*/
    00352   Vector2<double> operator*(const Vector2<double>& v) const {
    00353     return Vector2<double>(cosinus * v.x - sinus * v.y,sinus * v.x + cosinus * v.y);
    00354   }
    00355
    00356   /** concatenate to rotations
    00357    * @param other the other rotation
    00358    * @return the concatenation
    00359    */
    00360   RotationVector operator*(const RotationVector& other) const {
    00361     return RotationVector(cosinus * other.cosinus - sinus * other.sinus,
    00362                           sinus * other.cosinus + cosinus * other.sinus);
    00363   }
    00364
    00365   /** concatenate to rotations
    00366    * @param other the other rotation
    00367    * @return the concatenation
    00368    */
    00369   RotationVector operator+(const RotationVector& other) const {
    00370     return RotationVector(cosinus * other.cosinus - sinus * other.sinus,
    00371                           sinus * other.cosinus + cosinus * other.sinus);
    00372   }
    00373
    00374   /** concatenate to rotations (invers of second rotation)
    00375    * @param other the other rotation
    00376    * @return the concatenation
    00377    */
    00378   RotationVector operator-(const RotationVector& other) const {
    00379     return RotationVector(cosinus * other.cosinus + sinus * other.sinus,
    00380                           sinus * other.cosinus - cosinus * other.sinus);
    00381   }
    00382
    00383   /** concatenate to rotations (this= this*other)
    00384    * @param other the other rotation
    00385    * @return the concatenation
    00386    */
    00387   RotationVector operator*=(const RotationVector& other) {
    00388     return *this = RotationVector(cosinus * other.cosinus - sinus * other.sinus,
    00389                                   sinus * other.cosinus + cosinus * other.sinus);
    00390   }
    00391
    00392  /** concatenate to rotations (this= this*other)
    00393    * @param other the other rotation
    00394    * @return the concatenation
    00395    */
    00396   RotationVector operator+=(const RotationVector& other) {
    00397     return *this = RotationVector(cosinus * other.cosinus - sinus * other.sinus,
    00398                                   sinus * other.cosinus + cosinus * other.sinus);
    00399   }
    00400
    00401  /** concatenate to rotations (this= this*other)(second is inverted)
    00402    * @param other the other rotation
    00403    * @return the concatenation
    00404    */
    00405   RotationVector operator-=(const RotationVector& other) {
    00406     return *this = RotationVector(cosinus * other.cosinus + sinus * other.sinus,
    00407                                   sinus * other.cosinus - cosinus * other.sinus);
    00408   }
    00409
    00410
    00411   /** Comparison of two RotationVectors
    00412    * @param other the other RotationVector
    00413    * @return bool wether the RotationVectors are equal
    00414    */
    00415   bool operator==(const RotationVector& other) const {
    00416     return sinus == other.sinus && cosinus == other.cosinus;
    00417   }
    00418
    00419   /** Comparison of two RotationVectors
    00420    * @param other the other RotationVector
    00421    * @return bool wether the RotationVectors are unequal
    00422    */
    00423   bool operator!=(const RotationVector& other) const {
    00424     return sinus != other.sinus || cosinus != other.cosinus;
    00425   }
    00426
    00427   /** unary addition*/
    00428   RotationVector operator+() const {return *this;}
    00429
    00430   /** unary subtraction*/
    00431   RotationVector operator-() const {return RotationVector(cosinus,-sinus);}
    00432 };
    00433
    00434 /**
    00435 * Streaming operator that reads a RotationVector from a stream.
    00436 * @param stream The stream from which is read.
    00437 * @param rotationVector The RotationVector object.
    00438 * @return The stream.
    00439 */
    00440 In& operator>>(In& stream, RotationVector& rotationVector);
    00441
    00442 /**
    00443 * Streaming operator that writes a RotationVector to a stream.
    00444 * @param stream The stream to write on.
    00445 * @param rotationVector The RotationVector object.
    00446 * @return The stream.
    00447 */
    00448 Out& operator<<(Out& stream, const RotationVector& rotationVector);
    00449
    00450 /**
    00451  *
    00452  */
    00453 class Pose2D {
    00454   public:
    00455   /** the rotation*/
    00456   RotationVector rotation;
    00457
    00458   /** the translation*/
    00459   Vector2<double> translation;
    00460
    00461   /** noargs-constructor*/
    00462   Pose2D(){};
    00463
    00464   /** constructor from rotation and translation
    00465    * @param rot rotation (Angle)
    00466    * @param trans translation (Vector2)
    00467    */
    00468   /* vc++ bug, translation = trans is workaround */
    00469 //    Pose2D(const Angle& rot, const Vector2<double>& trans) {rotation = rot; translation=trans;}
    00470
    00471   /** constructor from rotation and translation
    00472    * @param rot rotation (RotationVector)
    00473    * @param trans translation (Vector2)
    00474    */
    00475   /* vc++ bug, translation = trans is workaround */
    00476   Pose2D(const RotationVector& rot, const Vector2<double>& trans) {rotation = rot; translation=trans;}
    00477
    00478   /** constructor from rotation and translation
    00479    * @param rot rotation (Angle)
    00480    * @param x translation.x (double)
    00481    * @param y translation.y (double)
    00482    */
    00483 /*   Pose2D(const Angle& rot, const double x, const double y) {
    00484     rotation = rot; translation.x = x, translation.y = y;
    00485   }*/
    00486
    00487   /** constructor from rotation and translation
    00488    * @param rot rotation (RotationVector)
    00489    * @param x translation.x (double)
    00490    * @param y translation.y (double)
    00491    */
    00492   Pose2D(const RotationVector& rot, const double x, const double y) {
    00493     rotation = rot; translation.x = x, translation.y = y;
    00494   }
    00495
    00496   /** constructor from rotation
    00497    * @param rot rotation (Angle)
    00498    */
    00499   /* vc++ bug, translation = trans is workaround */
    00500   Pose2D(const double rot) {rotation = rot;}
    00501
    00502   /** constructor from rotation
    00503    * @param rot rotation (RotationVector)
    00504    */
    00505   /* vc++ bug, translation = trans is workaround */
    00506   Pose2D(const RotationVector& rot) {rotation = rot;}
    00507
    00508   /** constructor from translation
    00509    * @param trans translation (Vector2)
    00510    */
    00511   /* vc++ bug, translation = trans is workaround */
    00512   Pose2D(const Vector2<double>& trans) {translation = trans;}
    00513
    00514   /** constructor from translation
    00515    * @param trans translation (Vector2)
    00516    */
    00517   /* vc++ bug, translation = trans is workaround */
    00518   Pose2D(const Vector2<int>& trans) {translation.x = trans.x; translation.y = trans.y;}
    00519
    00520
    00521   /** constructor from two translation values
    00522    * @param x translation x component
    00523    * @param y translation y component
    00524    */
    00525   Pose2D(const double x, const double y) {translation = Vector2<double>(x,y); }
    00526
    00527   /** Assignment operator
    00528    * @param other The other Pose2D that is assigned to this one
    00529    * @return A reference to this object after the assignment.
    00530    */
    00531   Pose2D& operator=(const Pose2D& other){
    00532     rotation = other.rotation;
    00533     translation = other.translation;
    00534     return *this;
    00535   }
    00536
    00537   /** Copy constructor
    00538    * @param other The other vector that is copied to this one
    00539    */
    00540   Pose2D(const Pose2D& other) {*this = other;}
    00541
    00542   /** get the Angle
    00543   * @return Angle the Angle which defines the rotation
    00544   */
    00545   double getAngle() const {return rotation.getAngle();}
    00546
    00547   /** set the Rotation from an angle
    00548   * @return the pose2D
    00549   */
    00550   Pose2D fromAngle(const double a) {rotation.fromAngle(a); return *this;}
    00551
    00552   /** get the Sinus of the Angle
    00553   * @return Sinus of the Angle the Angle which defines the rotation
    00554   */
    00555   double getSin() const {return rotation.sinus;}
    00556
    00557   /** get the Cosinus of the Angle
    00558   * @return Cosinus of the Angle the Angle which defines the rotation
    00559   */
    00560   double getCos() const {return rotation.sinus;}
    00561
    00562
    00563   /** Multiplication of a Vector2 with this Pose2D
    00564    * @param point The Vector2 that will be multiplicated with this Pose2D
    00565    * @return The resulting Vector2
    00566    */
    00567   Vector2<double> operator*(const Vector2<double>& point) const {
    00568     return translation + rotation * point;
    00569   }
    00570
    00571   /** Comparison of another pose with this one.
    00572    * @param other The other pose that will be compared to this one
    00573    * @return Whether the two poses are equal.
    00574    */
    00575   bool operator==(const Pose2D& other) const {
    00576     return translation == other.translation && rotation == other.rotation;
    00577   }
    00578
    00579   /** Comparison of another pose with this one.
    00580    * @param other The other pose that will be compared to this one
    00581    * @return Whether the two poses are unequal.
    00582    */
    00583   bool operator!=(const Pose2D& other) const {return !(*this == other);}
    00584
    00585   /**Concatenation of this pose with another pose.
    00586    * @param other The other pose that will be concatenated to this one.
    00587    * @return A reference to this pose after concatenation.
    00588    */
    00589   Pose2D& operator+=(const Pose2D& other) {
    00590     translation = *this * other.translation;
    00591     rotation *= other.rotation;
    00592     return *this;
    00593   }
    00594
    00595   /**A concatenation of this pose and another pose.
    00596    * @param other The other pose that will be concatenated to this one.
    00597    * @return The resulting pose.
    00598    */
    00599   Pose2D operator+(const Pose2D& other) const {
    00600     return Pose2D(*this) += other;
    00601   }
    00602
    00603   /**Difference of this pose relative to another pose.
    00604    * @param other The other pose that will be used as origin for the new pose.
    00605    * @return A reference to this pose after calculating the difference.
    00606    */
    00607   Pose2D& operator-=(const Pose2D& other) {
    00608     translation -= other.translation;
    00609     Pose2D p(-other.rotation);
    00610     return *this = p + *this;
    00611   }
    00612
    00613   /**Difference of this pose relative to another pose.
    00614    * @param other The other pose that will be used as origin for the new pose.
    00615    * @return The resulting pose.
    00616    */
    00617   Pose2D operator-(const Pose2D& other) const {
    00618     return Pose2D(*this) -= other;
    00619   }
    00620
    00621   /**Concatenation of this pose with another pose
    00622    * @param other The other pose that will be concatenated to this one.
    00623    * @return A reference to this pose after concatenation
    00624    */
    00625   Pose2D& conc(const Pose2D& other) {
    00626     return *this += other;
    00627   }
    00628
    00629   /**Translate this pose by a translation vector
    00630    * @param trans Vector to translate with
    00631    * @return A reference to this pose after translation
    00632    */
    00633   Pose2D& translate(const Vector2<double>& trans) {
    00634     translation = *this * trans;
    00635     return *this;
    00636   }
    00637
    00638   /**Translate this pose by a translation vector
    00639    * @param x x component of vector to translate with
    00640    * @param y y component of vector to translate with
    00641    * @return A reference to this pose after translation
    00642    */
    00643   Pose2D& translate(const double x, const double y) {
    00644     translation = *this * Vector2<double>(x,y);
    00645     return *this;
    00646   }
    00647
    00648   /**Rotate this pose by a rotation
    00649    * @param rot Angle to rotate with
    00650    * @return A reference to this pose after rotation
    00651    */
    00652   Pose2D& rotate(const double angle) {
    00653     rotation *= RotationVector(angle);
    00654     return *this;
    00655   }
    00656
    00657   /**Rotate this pose by a rotation
    00658    * @param rot RotationVector to rotate with
    00659    * @return A reference to this pose after rotation
    00660    */
    00661   Pose2D& rotate(const RotationVector& rot) {
    00662     rotation *= rot;
    00663     return *this;
    00664   }
    00665
    00666   /**
    00667   * The function creates a random pose.
    00668   * @param x The range for x-values of the pose.
    00669   * @param y The range for y-values of the pose.
    00670   * @param angle The range for the rotation of the pose.
    00671   */
    00672   static Pose2D random(const Range<double>& x,
    00673                        const Range<double>& y,
    00674                        const Range<double>& angle)
    00675   { // angle should even work in wrap around case!
    00676     return Pose2D(::random() * (angle.max - angle.min) + angle.min,
    00677                   Vector2<double>(::random() * (x.max - x.min) + x.min,
    00678                                   ::random() * (y.max - y.min) + y.min));
    00679   }
    00680 };
    00681
    00682 #endif //Pose2Dnew
    00683
    00684 /**
    00685 * Streaming operator that reads a Pose2D from a stream.
    00686 * @param stream The stream from which is read.
    00687 * @param pose2D The Pose2D object.
    00688 * @return The stream.
    00689 */
    00690 In& operator>>(In& stream, Pose2D& pose2D);
    00691
    00692 /**
    00693 * Streaming operator that writes a Pose2D to a stream.
    00694 * @param stream The stream to write on.
    00695 * @param pose2D The Pose2D object.
    00696 * @return The stream.
    00697 */
    00698 Out& operator<<(Out& stream, const Pose2D& pose2D);
    00699
    00700
    00701 /* *Concatenation of two 2D poses
    00702  * \param p1 pose1
    00703  * \param p2 pose2
    00704  * /
    00705 static Pose2D conc(const Pose2D& p1, const Pose2D& p2){
    00706   return Pose2D(p1.rotation + p2.rotation, p1 * p2.translation);
    00707 }
    00708 */
    00709
    00710 #endif // __Pose2D_h__