libalmath  2.1.4.13
 All Classes Namespaces Functions Variables Typedefs Groups Pages
alpose2d.h
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 
295  inline float norm() const
296  {
297  return std::sqrt(x * x + y * y);
298  }
299 
308  Pose2D normalize() const;
309 
316  inline float getAngle(void) const
317  {
318  return std::atan2(y, x);
319  }
320  }; // end struct
321 
322 
334  float distanceSquared(
335  const Pose2D& pPos1,
336  const Pose2D& pPos2);
337 
349  float distance(
350  const Pose2D& pPos1,
351  const Pose2D& pPos2);
352 
359  void pose2dInvertInPlace(Pose2D& pPos);
360 
367  Pose2D pinv(const Pose2D& pPos);
368 
369 
382  Pose2D pose2dDiff(
383  const Pose2D& pPos1,
384  const Pose2D& pPos2);
385 
392  Pose2D pose2DInverse(const Pose2D& pPos);
393 
400  void pose2DInverse(
401  const Pose2D& pPos,
402  Pose2D& pRes);
403 
404 
405  } // end namespace math
406 } // end namespace AL
407 #endif // _LIBALMATH_ALMATH_TYPES_ALPOSE2D_H_
float getAngle(void) const
Returns the angle of the current Pose2D.
Definition: alpose2d.h:316
Pose2D operator*(const Pose2D &pPos2) const
Overloading of operator * for Pose2D.
Definition: alpose2d.h:182
Pose2D()
Create a Pose2D initialized with 0.0f.
float distance(const Pose2D &pPos2) const
Compute the distance between the actual Pose2D and the one give in argument.
A pose in a 2-dimentional space.
Definition: alpose2d.h:25
float norm() const
Compute the norm of the current Pose2D.
Definition: alpose2d.h:295
Pose2D operator+(void) const
Overloading of operator + for Pose2D.
Definition: alpose2d.h:147
float distance(const Pose2D &pPos1, const Pose2D &pPos2)
Compute the distance between two Pose2D.
float distanceSquared(const Pose2D &pPos2) const
Compute the squared distance between the actual Pose2D and the one give in argument.
void pose2DInverse(const Pose2D &pPos, Pose2D &pRes)
Compute the inverse of a Pose2D.
Pose2D & operator*=(const Pose2D &pPos2)
Overloading of operator *= for Pose2D.
Pose2D & operator-=(const Pose2D &pPos2)
Overloading of operator -= for Pose2D.
Pose2D pinv(const Pose2D &pPos)
Alternative name for inverse: return the pose2d inverse of the given Pose2D.
Pose2D inverse() const
Return the inverse of the Pose2D
Pose2D operator-() const
Overloading of operator - for Pose2D.
Definition: alpose2d.h:155
Pose2D & operator+=(const Pose2D &pPos2)
Overloading of operator += for Pose2D.
Pose2D diff(const Pose2D &pPos2) const
Compute the Pose2D between the actual Pose2D and the one given in argument:
bool isNear(const Pose2D &pPos2, const float &pEpsilon=0.0001f) const
Check if the actual Pose2D is near the one given in argument.
bool operator==(const Pose2D &pPos2) const
Overloading of operator == for Pose2D.
Pose2D & operator/=(float pVal)
Overloading of operator /= for Pose2D.
void pose2dInvertInPlace(Pose2D &pPos)
Inverse the given Pose2D in place:
float distanceSquared(const Pose2D &pPos1, const Pose2D &pPos2)
Compute the squared distance between two Pose2D.
static Pose2D fromPolarCoordinates(const float pRadius, const float pAngle)
Create a Pose2D from polar coordinates.
Pose2D operator/(float pVal) const
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: ...
bool operator!=(const Pose2D &pPos2) const
Overloading of operator != for Pose2D.
Pose2D normalize() const
Normalize the current Pose2D position.