libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
altransformhelpers.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_ALTRANSFORMHELPERS_H_
10 #define _LIBALMATH_ALMATH_TOOLS_ALTRANSFORMHELPERS_H_
11 
20 #include <almath/types/alpose2d.h>
23 
24 namespace AL {
25  namespace Math {
26 
27 
31 
48  const Transform& pT,
49  Velocity6D& pVel);
50 
51 
55 
64  Velocity6D transformLogarithm(const Transform& pT);
71 
72 
74 
78  Transform velocityExponential(const Velocity6D& pVel);
85 
86  // TODO: Add to doc or set private.
88  const Velocity6D& pVel,
89  Transform& pT);
90 
92 
129  const Transform& pT,
130  const Velocity6D& pVelIn,
131  Velocity6D& pVelOut);
132 
134 
171  const Transform& pT,
172  const Position6D& pPosIn,
173  Position6D& pPosOut);
174 
175  // TODO: rename argument.
177  const Transform& pT,
178  Position3D& pPosOut);
179 
180  // TODO: rename argument.
182  const Transform& pT,
183  Position3D& pPosOut);
184 
186 
214  const Transform& pT,
215  const Position3D& pPosIn,
216  Position3D& pPosOut);
217 
219 
239  const Transform& pT,
240  const Position3D& pPosIn,
241  Position3D& pPosOut);
242 
244 
264  const Transform& pT,
265  const Transform& pTIn,
266  Transform& pTOut);
267 
269 
289  const Transform& pT,
290  const Transform& pTIn,
291  Transform& pTOut);
292 
293 
295 
332  const Transform& pT,
333  const Velocity6D& pVelIn,
334  Velocity6D& pVelOut);
335 
337 
374  const Transform& pT,
375  const Position6D& pPosIn,
376  Position6D& pPosOut);
377 
389  const Transform& pTIn1,
390  const Transform& pTIn2,
391  const float& pVal,
392  Transform& pTOut);
393 
405  Transform transformMean(
406  const Transform& pTIn1,
407  const Transform& pTIn2,
408  const float& pVal = 0.5f);
409 
412 
429  const Rotation& pRot,
430  const float& pX,
431  const float& pY,
432  const float& pZ);
433 
436 
450  const Rotation& pRot,
451  const Position3D& pPos);
452 
455 
471  const Position3D& pPosition,
472  Transform& pTransform);
473 
477 
488  Transform transformFromPosition3D(const Position3D& pPosition);
496 
500 
516  const Rotation& pRotation,
517  Transform& pTransform);
518 
520 
531  Transform transformFromRotation(const Rotation& pRotation);
538 
546  const Transform& pTransform,
547  Rotation& pRotation);
548 
557  Rotation rotationFromTransform(const Transform& pTransform);
558 
568  Rotation3D rotation3DFromRotation(const Rotation& pRotation);
569 
583  Rotation rotationFromAxesXY(const Position3D& pX, const Position3D& pY);
584 
598  Rotation rotationFromAxesXZ(const Position3D& pX, const Position3D& pZ);
599 
613  Rotation rotationFromAxesYZ(const Position3D& pY, const Position3D& pZ);
614 
628  Rotation rotationFromAxesXYZ(const Position3D& pX, const Position3D& pY, const Position3D& pZ);
629 
630 
638  const Transform& pT,
639  Position6D& pPos);
640 
647  Position6D position6DFromTransform(const Transform& pT);
648 
656  const Pose2D& pPose,
657  Transform& pT);
658 
665  Transform transformFromPose2D(const Pose2D& pPose);
666 
674  const Transform& pT,
675  Pose2D& pPos);
676 
683  Pose2D pose2DFromTransform(const Transform& pT);
684 
692  const Transform& pT,
693  Position2D& pPos);
694 
701  Position2D position2DFromTransform(const Transform& pT);
702 
711  Transform transformFromRotation3D(const Rotation3D& pRotation);
712 
719  Transform transformFromPosition6D(const Position6D& pPosition6D);
720 
740  const Transform& pCurrent,
741  const Transform& pTarget,
742  Position6D& result);
743 
753  Position6D position6DFromTransformDiff(
754  const Transform& pCurrent,
755  const Transform& pTarget);
756 
760 
779  const Transform& pT,
780  Position3D& pPos);
781 
785 
799  Position3D position3DFromTransform(const Transform& pT);
804 
805 
814  Rotation3D rotation3DFromTransform(const Transform& pT);
815 
825  const int pAxis,
826  const float pTheta,
827  const Position3D& pPos,
828  Transform& pT);
829 
837  Transform transformFromRotVec(
838  const int pAxis,
839  const float pTheta,
840  const Position3D& pPos);
841 
842 
849  const Position3D& pPos,
850  Transform& pT);
851 
857  Transform transformFromRotVec(const Position3D& pPos);
858 
863 
866  Transform transformFromRotVec(
867  const int& pAxis,
868  const float& pTheta);
869 
870  Position3D operator*(
871  const Transform& pT,
872  const Position2D& pPos);
873 
874  Position3D operator*(
875  const Transform& pT,
876  const Position3D& pPos);
877 
885  Transform axisRotationProjection(
886  const Position3D& pAxis,
887  const Transform& pT);
888 
896  Rotation axisRotationProjection(
897  const Position3D& pAxis,
898  const Rotation& pRot);
899 
900 
908  const Position3D& pAxis,
909  Transform& pT);
910 
918  const Position3D& pAxis,
919  Rotation& pRot);
920 
927  void orthogonalSpace(
928  const Position3D& pPos,
929  Transform& pTf);
930 
931  Transform orthogonalSpace(const Position3D& pPos);
932 
933  Transform transformFromQuaternion(
934  const Quaternion& pQua);
935 
936  Quaternion quaternionFromTransform(
937  const Transform& pT);
938 
945  Transform transformFromDisplacement(const Displacement& pDisp);
946 
953  Displacement displacementFromTransform(const Transform& pTrans);
954 
955  } // namespace Math
956 } // namespace AL
957 #endif // _LIBALMATH_ALMATH_TOOLS_ALTRANSFORMHELPERS_H_
Transform velocityExponential(const Velocity6D &pVel)
void changeReferenceTransposePosition3DInPlace(const Transform &pT, Position3D &pPosOut)
void position2DFromTransformInPlace(const Transform &pT, Position2D &pPos)
Compute a Position2D from a Transform.
Position2D position2DFromTransform(const Transform &pT)
Create a Position2D from a Transform.
void position6DFromTransformDiffInPlace(const Transform &pCurrent, const Transform &pTarget, Position6D &result)
Computes a 6 differential motion required to move from a 4*4 Homogenous transform matrix Current to a...
void transformMeanInPlace(const Transform &pTIn1, const Transform &pTIn2, const float &pVal, Transform &pTOut)
Preform a logarithmic mean of pTIn1 and pTIn2 and put it in pTout.
void changeReferenceTransform(const Transform &pT, const Transform &pTIn, Transform &pTOut)
Rotation rotationFromAxesYZ(const Position3D &pY, const Position3D &pZ)
return a Rotation Matrix initialized with its direction vectors X and Y in world coordinates. These vectors represent, respectively, the second and third columns of the Rotation matrix. The vectors must be unitary and orthogonal.
void transformFromRotationInPlace(const Rotation &pRotation, Transform &pTransform)
Modify the rotation part of the transform. The translation part of the transform is not modified...
void position6DFromTransformInPlace(const Transform &pT, Position6D &pPos)
Compute Position6D corresponding to the Transform.
void transformFromPose2DInPlace(const Pose2D &pPose, Transform &pT)
Compute a Transform from a Pose2D.
Transform transformFromRotation(const Rotation &pRotation)
Displacement displacementFromTransform(const Transform &pTrans)
void changeReferencePosition3D(const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)
void velocityExponentialInPlace(const Velocity6D &pVel, Transform &pT)
void changeReferencePosition6D(const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)
Rotation rotationFromTransform(const Transform &pTransform)
Rotation3D rotation3DFromRotation(const Rotation &pRotation)
return 3 angles which result in the equivalent rotation when composed in the following order: Rz(wz) ...
Transform transformFromPosition3D(const Position3D &pPosition)
Create a 4*4 transform matrix from cartesian coordinates given in pPosition.
Transform transformMean(const Transform &pTIn1, const Transform &pTIn2, const float &pVal=0.5f)
Preform a logarithmic mean of pTIn1 and pTIn2.
Rotation3D rotation3DFromTransform(const Transform &pT)
Create a Rotation3D (Roll, Pitch, Yaw) corresponding to the rotational part of the Transform...
void changeReferenceTransposeTransform(const Transform &pT, const Transform &pTIn, Transform &pTOut)
void transformFromPosition3DInPlace(const Position3D &pPosition, Transform &pTransform)
Modify pTransform to set the translation part to pPosition.
Transform transformFromPosition6D(const Position6D &pPosition6D)
Create a Transform from a Position6D.
void axisRotationProjectionInPlace(const Position3D &pAxis, Transform &pT)
void position3DFromTransformInPlace(const Transform &pT, Position3D &pPos)
Compute a Position3D from a Transform.
void transformLogarithmInPlace(const Transform &pT, Velocity6D &pVel)
Compute the logarithme of a transform. Angle must be between
Transform transformFromRotation3D(const Rotation3D &pRotation)
Create a Transform from the 3 angles stored in a Rotation3D. The angles are composed in the following...
Transform axisRotationProjection(const Position3D &pAxis, const Transform &pT)
void changeReferenceTransposePosition6D(const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)
void changeReferenceTransposeVelocity6D(const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)
Transform transformFromDisplacement(const Displacement &pDisp)
Transform transformFromPose2D(const Pose2D &pPose)
Create a Transform from a Pose2D.
void changeReferencePosition3DInPlace(const Transform &pT, Position3D &pPosOut)
void rotationFromTransformInPlace(const Transform &pTransform, Rotation &pRotation)
Extract the position coordinates from a Transform.
Quaternion quaternionFromTransform(const Transform &pT)
void changeReferenceVelocity6D(const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)
Velocity6D transformLogarithm(const Transform &pT)
Compute the logarithme of a transform. Angle must be between
Pose2D pose2DFromTransform(const Transform &pT)
Create a Pose2D from a Transform.
Transform transformFromRotationPosition3D(const Rotation &pRot, const float &pX, const float &pY, const float &pZ)
Create a Transform from 3D cartesian coordiantes and a Rotation.
Position3D position3DFromTransform(const Transform &pT)
Create a Position3D from a Transform.
Position6D position6DFromTransformDiff(const Transform &pCurrent, const Transform &pTarget)
Computes a 6 differential motion require to move from a 4*4 Homogenous transform matrix Current to a ...
Position6D position6DFromTransform(const Transform &pT)
Compute Position6D corresponding to 4*4 Homogenous Transform.
void changeReferenceTransposePosition3D(const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)
Transform transformFromQuaternion(const Quaternion &pQua)
Transform transformFromRotVec(const int pAxis, const float pTheta, const Position3D &pPos)
Rotation rotationFromAxesXY(const Position3D &pX, const Position3D &pY)
return a Rotation Matrix initialized with its direction vectors X and Y in world coordinates. These vectors represent, respectively, the first and second columns of the Rotation matrix. The vectors must be unitary and orthogonal.
Rotation rotationFromAxesXZ(const Position3D &pX, const Position3D &pZ)
return a Rotation Matrix initialized with its direction vectors X and Z in world coordinates. These vectors represent, respectively, the first and third columns of the Rotation matrix. The vectors must be unitary and orthogonal.
void orthogonalSpace(const Position3D &pPos, Transform &pTf)
Position3D operator*(const Rotation &pRot, const Position3D &pPos)
Overloading of operator * between Rotation and Position3D:
void transformFromRotVecInPlace(const int pAxis, const float pTheta, const Position3D &pPos, Transform &pT)
Compute a Transform from
void pose2DFromTransformInPlace(const Transform &pT, Pose2D &pPos)
Compute a Pose2D from a Transform.
Rotation rotationFromAxesXYZ(const Position3D &pX, const Position3D &pY, const Position3D &pZ)
return a Rotation Matrix initialized with its direction vectors in world coordinates. The vectors represent the columns of the Rotation matrix. The vectors must be unitary and orthogonal.