libalmath  2.5.11.14a
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
alposition6d.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_ALPOSITION6D_H_
10 #define _LIBALMATH_ALMATH_TYPES_ALPOSITION6D_H_
11 
12 #include <vector>
13 
14 namespace AL {
15  namespace Math {
16 
23  struct Position6D {
25  float x;
27  float y;
29  float z;
31  float wx;
33  float wy;
35  float wz;
36 
39 
58  Position6D();
60 
63 
82  explicit Position6D(float pInit);
86 
89 
108  Position6D(
116  float pX,
117  float pY,
118  float pZ,
119  float pWx,
120  float pWy,
121  float pWz);
122 
125 
144  Position6D(const std::vector<float>& pFloats);
150 
155  inline Position6D operator+ (const Position6D& pPos2) const
156  {
157  return Position6D(
158  x + pPos2.x,
159  y + pPos2.y,
160  z + pPos2.z,
161  wx + pPos2.wx,
162  wy + pPos2.wy,
163  wz + pPos2.wz);
164  }
165 
170  inline Position6D operator- (const Position6D& pPos2) const
171  {
172  return Position6D(
173  x - pPos2.x,
174  y - pPos2.y,
175  z - pPos2.z,
176  wx - pPos2.wx,
177  wy - pPos2.wy,
178  wz - pPos2.wz);
179  }
180 
184  inline Position6D operator+ (void) const
185  {
186  return *this;
187  }
188 
192  inline Position6D operator- () const
193  {
194  return Position6D(-x, -y, -z, -wx, -wy, -wz);
195  }
196 
201  Position6D& operator+= (const Position6D& pPos2);
202 
207  Position6D& operator-= (const Position6D& pPos2);
208 
213  bool operator== (const Position6D& pPos2) const;
214 
219  bool operator!= (const Position6D& pPos2) const;
220 
225  inline Position6D operator* (float pVal) const
226  {
227  return Position6D(
228  x * pVal,
229  y * pVal,
230  z * pVal,
231  wx * pVal,
232  wy * pVal,
233  wz * pVal);
234  }
235 
240  Position6D operator/ (float pVal) const;
241 
246  Position6D& operator*= (float pVal);
247 
252  Position6D& operator/= (float pVal);
253 
265  bool isNear(
266  const Position6D& pPos2,
267  const float& pEpsilon=0.0001f) const;
268 
279  float distanceSquared(const Position6D& pPos2) const;
280 
291  float distance(const Position6D& pPos2) const;
292 
301  float norm() const;
302 
306  void toVector (std::vector<float>& pReturnVector) const;
307  std::vector<float> toVector (void) const;
308 
313  void writeToVector(std::vector<float>::iterator& pIt) const;
314 
315  }; // end struct
316 
317 
330  float distanceSquared(
331  const Position6D& pPos1,
332  const Position6D& pPos2);
333 
334 
346  float distance(
347  const Position6D& pPos1,
348  const Position6D& pPos2);
349 
360  float norm(const Position6D& pPos);
361 
372  Position6D normalize(const Position6D& pPos);
373 
374  } // end namespace math
375 } // end namespace al
376 
377 #endif // _LIBALMATH_ALMATH_TYPES_ALPOSITION6D_H_
Position6D operator-() const
Overloading of operator - for Position6D.
Definition: alposition6d.h:192
void writeToVector(std::vector< float >::iterator &pIt) const
Write [x, y, z, wx, wy, wz] in the vector and update the iterator. It is assumed the vector has enoug...
float distanceSquared(const Position6D &pPos2) const
Compute the squared distance of translation part (x, y and z) between the actual Position6D and the o...
bool isNear(const Position6D &pPos2, const float &pEpsilon=0.0001f) const
Check if the actual Position6D is near the one given in argument.
float norm(const Position2D &pPos)
Compute the norm of a Position2D.
Position6D & operator/=(float pVal)
Overloading of operator /= for Position6D.
Position6D operator+(void) const
Overloading of operator + for Position6D.
Definition: alposition6d.h:184
float distance(const Position6D &pPos2) const
Compute the distance of translation part (x, y and z) between the actual Position6D and the one give ...
float distanceSquared(const Pose2D &pPos1, const Pose2D &pPos2)
Compute the squared distance between two Pose2D.
Create and play with a Position6D.
Definition: alposition6d.h:23
std::vector< float > toVector(void) const
bool operator==(const Position6D &pPos2) const
Overloading of operator == for Position6D.
float distance(const Pose2D &pPos1, const Pose2D &pPos2)
Compute the distance between two Pose2D.
Position2D normalize(const Position2D &pPos)
Normalize a Position2D.
Position6D()
Create a Position6D initialized with 0.0f.
Position6D operator/(float pVal) const
Overloading of operator / for Position6D.
Position6D operator*(float pVal) const
Overloading of operator * for Position6D.
Definition: alposition6d.h:225
Position6D & operator*=(float pVal)
Overloading of operator *= for Position6D.
Position6D & operator-=(const Position6D &pPos2)
Overloading of operator -= for Position6D.
Position6D & operator+=(const Position6D &pPos2)
Overloading of operator += for Position6D.
float norm() const
Compute the norm of the actual Position6D:
bool operator!=(const Position6D &pPos2) const
Overloading of operator != for Position6D.