libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
almath.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_TOOLS_ALMATH_H_
10 #define _LIBALMATH_ALMATH_TOOLS_ALMATH_H_
11 
20 #include <almath/types/alpose2d.h>
21 
22 namespace AL {
23  namespace Math {
24 
31  void modulo2PIInPlace(float& pAngle);
32 
42  float modulo2PI(float pAngle);
43 
55  float meanAngle(const std::vector<float>& pAngles);
56 
72  float weightedMeanAngle(const std::vector<float>& pAngles,
73  const std::vector<float>& pWeights);
74 
88  bool clipData(
89  const float& pMin,
90  const float& pMax,
91  float& pData);
92 
93  bool clipData(
94  const float& pMin,
95  const float& pMax,
96  std::vector<float>& pData);
97 
98  bool clipData(
99  const float& pMin,
100  const float& pMax,
101  std::vector<std::vector<float> >& pData);
102 
104 
129  const float& pTheta,
130  const Pose2D& pPosIn,
131  Pose2D& pPosOut);
132 
140  const float& pTheta,
141  Pose2D& pPosOut);
142 
146 
155  Position6D position6DFromVelocity6D(const Velocity6D& pVel);
163 
172  const Pose2D& pPose2D,
173  Position2D& pPosition2D);
174 
178 
183  Position2D position2DFromPose2D(const Pose2D& pPose2D);
191 
195 
201  Position3D position3DFromPosition6D(const Position6D& pPosition6D);
209 
213 
229  Position3D operator*(
238  const Rotation& pRot,
239  const Position3D& pPos);
240 
250  Position3D operator*(
251  const Quaternion& pQuat,
252  const Position3D& pPos);
253 
254 
258 
267  Velocity6D operator*(
276  const float pVal,
277  const Position6D& pPos);
278 
282 
288  Velocity3D operator*(
297  const float pVal,
298  const Position3D& pPos);
299 
303 
319  Velocity3D operator*(
328  const Rotation& pRot,
329  const Velocity3D& pVel);
330 
341  const float& pTheta,
342  const Position3D& pPos);
343 
344 
352  const Pose2D& pPose2D,
353  Position6D& pPosition6D);
354 
361  Position6D position6DFromPose2D(const Pose2D& pPose2D);
362 
370  const Position3D& pPosition3D,
371  Position6D& pPosition6D);
372 
379  Position6D position6DFromPosition3D(const Position3D& pPosition3D);
380 
388  const Position6D& pPosition6D,
389  Pose2D& pPose2D);
390 
397  Pose2D pose2DFromPosition6D(const Position6D& pPosition6D);
398 
410  const Position2D& pPosition2D,
411  const float pAngle,
412  Pose2D& pPose2D);
413 
421  Pose2D pose2DFromPosition2D(const Position2D& pPosition2D,
422  const float pAngle=0.0f);
423 
427 
432  Position2D operator*(
441  const Pose2D& pVal,
442  const Position2D& pPos);
443 
452  const Rotation3D& pRot3D,
453  Quaternion& pQuaternion);
454 
455  Quaternion quaternionFromRotation3D(
456  const Rotation3D& pRot3D);
457 
465  const Quaternion& pQua,
466  Rotation& pRot);
467 
468  Rotation rotationFromQuaternion(
469  const Quaternion& pQua);
470 
479  const Quaternion& pQuaterion,
480  Rotation3D& pRot3D);
481 
482  Rotation3D rotation3DFromQuaternion(
483  const Quaternion& pQuaternion);
484 
493  const Position6D& pPos6D,
494  Quaternion& pQua,
495  Position3D& pPos3D);
496 
497 
501 
533  float pMass,
534  const Position3D& pPos,
535  std::vector<float>& pInertia);
536  } // namespace Math
537 } // namespace AL
538 #endif // _LIBALMATH_ALMATH_TOOLS_ALMATH_H_
void pose2DFromPosition6DInPlace(const Position6D &pPosition6D, Pose2D &pPose2D)
Compute a Pose2D from a Position6D.
void changeReferencePose2DInPlace(const float &pTheta, Pose2D &pPosOut)
Change orientation of a Pose2D in place.
float meanAngle(const std::vector< float > &pAngles)
Returns the mean of given angles (between ]-PI, PI]). It is defined by the direction of the mean of a...
void quaternionFromRotation3D(const Rotation3D &pRot3D, Quaternion &pQuaternion)
Create a Quaternion from a Rotation3D when composed in the following order: Rz(wz) * Ry(wy) * Rx(wx) ...
Position6D position6DFromVelocity6D(const Velocity6D &pVel)
Create a Position6D from a Velocity6D
Pose2D pose2DFromPosition6D(const Position6D &pPosition6D)
Create a Pose2D from a Position6D.
void pose2DFromPosition2DInPlace(const Position2D &pPosition2D, const float pAngle, Pose2D &pPose2D)
Compute a Pose2D from a Position2D. pPose2D.x = pPosition2D.x pPose2D.y = pPosition2D.y pPose2D.theta = pAngle
bool clipData(const float &pMin, const float &pMax, float &pData)
Clip an input data inside min and max limit.
void position6DFromPose2DInPlace(const Pose2D &pPose2D, Position6D &pPosition6D)
Compute a Position6D from a Pose2D.
void modulo2PIInPlace(float &pAngle)
Set an angle between ]-PI, PI].
float weightedMeanAngle(const std::vector< float > &pAngles, const std::vector< float > &pWeights)
Returns the weighted mean of given angles (between ]-PI, PI]). It is defined by the direction of the ...
void position6DFromPosition3DInPlace(const Position3D &pPosition3D, Position6D &pPosition6D)
Compute a Position6D from a Position3D.
Rotation rotationFromAngleDirection(const float &pTheta, const Position3D &pPos)
Creates a 3*3 Rotation Matrix from a an angle and a normalized Position3D.
Position2D position2DFromPose2D(const Pose2D &pPose2D)
Create a Position2D from a Pose2D
Position3D position3DFromPosition6D(const Position6D &pPosition6D)
Create a Position3D from a Position6D
Pose2D pose2DFromPosition2D(const Position2D &pPosition2D, const float pAngle=0.0f)
Create a Pose2D from a Position2D.
Position6D position6DFromPose2D(const Pose2D &pPose2D)
Create a Position6D from a Pose2D.
void changeReferencePose2D(const float &pTheta, const Pose2D &pPosIn, Pose2D &pPosOut)
void quaternionPosition3DFromPosition6D(const Position6D &pPos6D, Quaternion &pQua, Position3D &pPos3D)
Convert a Position6D to Quaternion and Position3D
Position6D position6DFromPosition3D(const Position3D &pPosition3D)
Create a Position6D from a Position3D.
void rotationFromQuaternion(const Quaternion &pQua, Rotation &pRot)
Create a Rotation Matrix from a Quaternion
void pointMassRotationalInertia(float pMass, const Position3D &pPos, std::vector< float > &pInertia)
Return the rotational inertia, expressed at the origin, of a point mass located at pPos...
void position2DFromPose2DInPlace(const Pose2D &pPose2D, Position2D &pPosition2D)
Compute a Position2D from a Pose2D. The theta member of the Pose2D is not taken into account...
Position3D operator*(const Rotation &pRot, const Position3D &pPos)
Overloading of operator * between Rotation and Position3D:
float modulo2PI(float pAngle)
Return an angle between ]-PI, PI].
void rotation3DFromQuaternion(const Quaternion &pQuaterion, Rotation3D &pRot3D)
Create a Rotation3D from a Quaternion when composed in the following order: Rz(wz) * Ry(wy) * Rx(wx) ...