libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
alpose2d.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
3  * Use of this source code is governed by a BSD-style license that can be
4  * found in the COPYING file.
5  */
6 
7 
8 #pragma once
9 #ifndef _LIBALMATH_ALMATH_TYPES_ALPOSE2D_H_
10 #define _LIBALMATH_ALMATH_TYPES_ALPOSE2D_H_
11 
12 #include <vector>
13 #include <cmath>
14 
15 namespace AL {
16  namespace Math {
17 
25  struct Pose2D {
27  float x;
29  float y;
31  float theta;
32 
35 
48  Pose2D();
50 
53 
66  explicit Pose2D(float pInit);
69 
72 
85  explicit Pose2D(
90  float pX,
91  float pY,
92  float pTheta);
93 
96 
109  Pose2D(const std::vector<float>& pFloats);
116 
124  static Pose2D fromPolarCoordinates(const float pRadius, const float pAngle);
125 
130  inline Pose2D operator+ (const Pose2D& pPos2) const
131  {
132  return Pose2D(x + pPos2.x, y + pPos2.y, theta + pPos2.theta);
133  }
134 
139  inline Pose2D operator- (const Pose2D& pPos2) const
140  {
141  return Pose2D(x - pPos2.x, y - pPos2.y, theta - pPos2.theta);
142  }
143 
147  inline Pose2D operator+ (void) const
148  {
149  return *this;
150  }
151 
155  inline Pose2D operator- () const
156  {
157  return Pose2D(-x, -y, -theta);
158  }
159 
164  Pose2D& operator+= (const Pose2D& pPos2);
165 
170  Pose2D& operator-= (const Pose2D& pPos2);
171 
176  Pose2D& operator*= (const Pose2D& pPos2);
177 
182  inline Pose2D operator* (const Pose2D& pPos2) const
183  {
184  return Pose2D(
185  x + std::cos(theta) * pPos2.x - std::sin(theta) * pPos2.y,
186  y + std::sin(theta) * pPos2.x + std::cos(theta) * pPos2.y,
187  theta + pPos2.theta);
188  }
189 
194  bool operator==(const Pose2D& pPos2) const;
195 
200  bool operator!=(const Pose2D& pPos2) const;
201 
206  inline Pose2D operator* (float pVal) const
207  {
208  return Pose2D(x*pVal, y*pVal, theta*pVal);
209  }
210 
215  Pose2D operator/ (float pVal) const;
216 
221  Pose2D& operator*= (float pVal);
222 
227  Pose2D& operator/= (float pVal);
228 
241  float distanceSquared(const Pose2D& pPos2) const;
242 
253  float distance(const Pose2D& pPos2) const;
254 
258  Pose2D inverse() const;
259 
268  Pose2D diff(const Pose2D& pPos2) const;
269 
280  bool isNear(const Pose2D& pPos2,
281  const float& pEpsilon=0.0001f) const;
282 
286  void toVector(std::vector<float>& pReturnVector) const;
287  std::vector<float> toVector(void) const;
288 
293  void writeToVector(std::vector<float>::iterator& pIt) const;
294 
301  inline float norm() const
302  {
303  return std::sqrt(x * x + y * y);
304  }
305 
314  Pose2D normalize() const;
315 
322  inline float getAngle(void) const
323  {
324  return std::atan2(y, x);
325  }
326  }; // end struct
327 
328 
340  float distanceSquared(
341  const Pose2D& pPos1,
342  const Pose2D& pPos2);
343 
355  float distance(
356  const Pose2D& pPos1,
357  const Pose2D& pPos2);
358 
365  void pose2dInvertInPlace(Pose2D& pPos);
366 
373  Pose2D pinv(const Pose2D& pPos);
374 
375 
388  Pose2D pose2dDiff(
389  const Pose2D& pPos1,
390  const Pose2D& pPos2);
391 
398  Pose2D pose2DInverse(const Pose2D& pPos);
399 
406  void pose2DInverse(
407  const Pose2D& pPos,
408  Pose2D& pRes);
409 
410 
411  } // end namespace math
412 } // end namespace AL
413 #endif // _LIBALMATH_ALMATH_TYPES_ALPOSE2D_H_
bool isNear(const Pose2D &pPos2, const float &pEpsilon=0.0001f) const
Check if the actual Pose2D is near the one given in argument.
static Pose2D fromPolarCoordinates(const float pRadius, const float pAngle)
Create a Pose2D from polar coordinates.
void pose2dInvertInPlace(Pose2D &pPos)
Inverse the given Pose2D in place:
Pose2D()
Create a Pose2D initialized with 0.0f.
bool operator!=(const Pose2D &pPos2) const
Overloading of operator != for Pose2D.
Pose2D & operator*=(const Pose2D &pPos2)
Overloading of operator *= for Pose2D.
float distanceSquared(const Pose2D &pPos2) const
Compute the squared distance between the actual Pose2D and the one give in argument.
Pose2D operator+(void) const
Overloading of operator + for Pose2D.
Definition: alpose2d.h:147
float norm() const
Compute the norm of the current Pose2D.
Definition: alpose2d.h:301
Pose2D operator*(const Pose2D &pPos2) const
Overloading of operator * for Pose2D.
Definition: alpose2d.h:182
Pose2D & operator-=(const Pose2D &pPos2)
Overloading of operator -= for Pose2D.
bool operator==(const Pose2D &pPos2) const
Overloading of operator == for Pose2D.
float distance(const Pose2D &pPos2) const
Compute the distance between the actual Pose2D and the one give in argument.
Pose2D normalize() const
Normalize the current Pose2D position.
void writeToVector(std::vector< float >::iterator &pIt) const
Write [x, y, theta] in the vector and update the iterator. It is assumed the vector has enough space...
float distanceSquared(const Pose2D &pPos1, const Pose2D &pPos2)
Compute the squared distance between two Pose2D.
Pose2D & operator+=(const Pose2D &pPos2)
Overloading of operator += for Pose2D.
Pose2D pose2dDiff(const Pose2D &pPos1, const Pose2D &pPos2)
Compute the Pose2D between the actual Pose2D and the one give in argument result: ...
Pose2D operator/(float pVal) const
Overloading of operator / for Pose2D.
float distance(const Pose2D &pPos1, const Pose2D &pPos2)
Compute the distance between two Pose2D.
float getAngle(void) const
Returns the angle of the current Pose2D.
Definition: alpose2d.h:322
Pose2D operator-() const
Overloading of operator - for Pose2D.
Definition: alpose2d.h:155
A pose in a 2-dimentional space.
Definition: alpose2d.h:25
Pose2D inverse() const
Return the inverse of the Pose2D
Pose2D & operator/=(float pVal)
Overloading of operator /= for Pose2D.
Pose2D diff(const Pose2D &pPos2) const
Compute the Pose2D between the actual Pose2D and the one given in argument:
std::vector< float > toVector(void) const
Pose2D pose2DInverse(const Pose2D &pPos)
Compute the inverse of a Pose2D.
Pose2D pinv(const Pose2D &pPos)
Alternative name for inverse: return the pose2d inverse of the given Pose2D.