libalmath  2.5.11.14a
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Namespaces | Classes | Typedefs | Functions
AL::Math Namespace Reference

Namespaces

 DSP
 

Classes

class  Shape3D
 
class  Sphere
 
class  RoundedRectangle
 
class  Pill
 
class  Plane
 
class  HalfSpace
 
class  Rectangle
 
class  HalfLine
 
class  Shape3DVisitor
 
class  NotImplementedShape3DVisitor
 
struct  BodyMass
 
struct  Displacement
 Struct composed of a Position3D and a Quaternion More...
 
struct  Pose2D
 A pose in a 2-dimentional space. More...
 
struct  Position2D
 Create and play with a Position2D. More...
 
struct  Position3D
 Create and play with a Position3D. More...
 
struct  Position6D
 Create and play with a Position6D. More...
 
struct  PositionAndVelocity
 Create and play with a PositionAndVelocity. More...
 
struct  Quaternion
 Create and play with a Quaternion. More...
 
struct  Rotation
 A 3*3 rotation matrix. More...
 
struct  Rotation3D
 A Rotation3D give 3 composed angles in radians. More...
 
struct  Transform
 A homogenous transformation matrix. More...
 
struct  TransformAndVelocity6D
 Struct composed of a Transform and a Velocity6D More...
 
struct  Velocity3D
 Create and play with a Velocity3D. More...
 
struct  Velocity6D
 Create and play with a Velocity6D. More...
 

Typedefs

typedef Eigen::Matrix< float, 3, 4 > Matrix34f
 
typedef Eigen::Matrix< float,
3, 4, Eigen::RowMajor > 
Matrix34frm
 
typedef Eigen::Matrix< float, 6, 1 > Vector6f
 
typedef std::bitset< 6 > AXIS_MASK
 Definition of an AXIS_MASK as a bit set. More...
 

Functions

void getSphereCentersFromPill (float pHalfExtent, const Math::Transform &pEndTfToPillCenter, Math::Position3D &pCenterA, Math::Position3D &pCenterB)
 
void computePillParameters (const Math::Position3D &pEndTfToCenterA, const Math::Position3D &pEndTfToCenterB, float &pHalfExtent, Math::Transform &pEndTfToPillCenter)
 
Matrix34f toEigenMatrix34 (const AL::Math::Transform &tr)
 
Eigen::Matrix3f toEigenMatrix3 (const AL::Math::Transform &tr)
 
Eigen::Vector3f toEigenVector3 (const AL::Math::Position3D &v)
 
Eigen::AffineCompact3f toEigenAffineCompact3 (const AL::Math::Transform &tr)
 
template<typename Derived0 >
void toALMathTransform (const Eigen::MatrixBase< Derived0 > &in, Transform &out)
 
template<typename Derived0 >
Transform toALMathTransform (const Eigen::MatrixBase< Derived0 > &m)
 
template<int _Mode, int _Options>
void toALMathTransform (const Eigen::Transform< float, 3, _Mode, _Options > &in, Transform &out)
 
template<int _Mode, int _Options>
Transform toALMathTransform (const Eigen::Transform< float, 3, _Mode, _Options > &tr)
 
template<typename Derived0 >
Position3D toALMathPosition3D (const Eigen::MatrixBase< Derived0 > &in)
 
template<typename Derived0 >
void toALMathVelocity6D (const Eigen::MatrixBase< Derived0 > &in, Velocity6D &out)
 
template<typename Derived0 >
Velocity6D toALMathVelocity6D (const Eigen::MatrixBase< Derived0 > &in)
 
ros::Time toValidRosTime (qi::Duration time_since_epoch)
 
qi::Duration toQiDuration (ros::Time time)
 
Eigen::Quaterniond eigenQuaternionFromUrdfRpy (const AL::urdf::Array3d &rpy)
 
template<typename T >
AL::urdf::Array3d urdfRpyFromEigenMatrix3 (const T &t)
 
Eigen::Isometry3d toEigenTransform (const AL::urdf::Pose &p)
 
Eigen::Matrix3d toEigenMatrix3 (const AL::urdf::Inertial &inertial)
 
template<typename Scalar , typename Derived0 , typename Derived1 >
void addPointMassInertia (Scalar mass, const Eigen::MatrixBase< Derived0 > &position, Eigen::MatrixBase< Derived1 > &output)
 
template<typename Scalar , typename EPose , typename Derived1 >
void squashInertial (Scalar mass_a, const EPose &pose_a, const Eigen::MatrixBase< Derived1 > &inertia_a, Scalar mass_b, const EPose &pose_b, const Eigen::MatrixBase< Derived1 > &inertia_b, Scalar &mass_c, EPose &pose_c, Eigen::MatrixBase< Derived1 > &inertia_c)
 
template<typename Scalar , typename Derived0 , typename Derived1 >
void urdfInertialToEigen (const AL::urdf::Inertial &inertial, Scalar &mass, Eigen::MatrixBase< Derived0 > &com_a, Eigen::MatrixBase< Derived1 > &inertia_a)
 
std::vector< Pose2DgetDubinsSolutions (const Pose2D &pTargetPose, const float pCircleRadius)
 Get the dubins solutions. More...
 
void modulo2PIInPlace (float &pAngle)
 Set an angle between ]-PI, PI]. More...
 
float modulo2PI (float pAngle)
 Return an angle between ]-PI, PI]. More...
 
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 all unitary vectors corresponding to the given angles. If that mean vector is a null vector, then the angular mean is not defined and the method will throw. For example, meanAngle([0, PI]) throws. More...
 
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 mean of all unitary vectors corresponding to the given angles, multiplied by the given weights. If that mean vector is a null vector, then the angular mean is not defined and the method will throw. For example, meanAngle([0, PI], [1.0, 1.0]) throws. More...
 
bool clipData (const float &pMin, const float &pMax, float &pData)
 Clip an input data inside min and max limit. More...
 
bool clipData (const float &pMin, const float &pMax, std::vector< float > &pData)
 
bool clipData (const float &pMin, const float &pMax, std::vector< std::vector< float > > &pData)
 
void changeReferencePose2D (const float &pTheta, const Pose2D &pPosIn, Pose2D &pPosOut)
 
void changeReferencePose2DInPlace (const float &pTheta, Pose2D &pPosOut)
 Change orientation of a Pose2D in place. More...
 
Position6D position6DFromVelocity6D (const Velocity6D &pVel)
 Create a Position6D from a Velocity6D More...
 
void position2DFromPose2DInPlace (const Pose2D &pPose2D, Position2D &pPosition2D)
 Compute a Position2D from a Pose2D. The theta member of the Pose2D is not taken into account. More...
 
Position2D position2DFromPose2D (const Pose2D &pPose2D)
 Create a Position2D from a Pose2D More...
 
Position3D position3DFromPosition6D (const Position6D &pPosition6D)
 Create a Position3D from a Position6D More...
 
Position3D operator* (const Rotation &pRot, const Position3D &pPos)
 Overloading of operator * between Rotation and Position3D: More...
 
Position3D operator* (const Quaternion &pQuat, const Position3D &pPos)
 Overloading of operator * between Quaternion and Position3D More...
 
Velocity6D operator* (const float pVal, const Position6D &pPos)
 Overloading of operator * for float to Position6D, give a Velocity6D: More...
 
Velocity3D operator* (const float pVal, const Position3D &pPos)
 Overloading of operator * for float to Position3D, give a Velocity3D: More...
 
Velocity3D operator* (const Rotation &pRot, const Velocity3D &pVel)
 Overloading of operator * between Rotation and Velocity3D: More...
 
Rotation rotationFromAngleDirection (const float &pTheta, const Position3D &pPos)
 Creates a 3*3 Rotation Matrix from a an angle and a normalized Position3D. More...
 
void position6DFromPose2DInPlace (const Pose2D &pPose2D, Position6D &pPosition6D)
 Compute a Position6D from a Pose2D. More...
 
Position6D position6DFromPose2D (const Pose2D &pPose2D)
 Create a Position6D from a Pose2D. More...
 
void position6DFromPosition3DInPlace (const Position3D &pPosition3D, Position6D &pPosition6D)
 Compute a Position6D from a Position3D. More...
 
Position6D position6DFromPosition3D (const Position3D &pPosition3D)
 Create a Position6D from a Position3D. More...
 
void pose2DFromPosition6DInPlace (const Position6D &pPosition6D, Pose2D &pPose2D)
 Compute a Pose2D from a Position6D. More...
 
Pose2D pose2DFromPosition6D (const Position6D &pPosition6D)
 Create a Pose2D from a Position6D. More...
 
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 More...
 
Pose2D pose2DFromPosition2D (const Position2D &pPosition2D, const float pAngle=0.0f)
 Create a Pose2D from a Position2D. More...
 
Position2D operator* (const Pose2D &pVal, const Position2D &pPos)
 Overloading of operator * for Pose2D to Position2D, give a Position2D: More...
 
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) More...
 
Quaternion quaternionFromRotation3D (const Rotation3D &pRot3D)
 
void rotationFromQuaternion (const Quaternion &pQua, Rotation &pRot)
 Create a Rotation Matrix from a Quaternion More...
 
Rotation rotationFromQuaternion (const Quaternion &pQua)
 
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) More...
 
Rotation3D rotation3DFromQuaternion (const Quaternion &pQuaternion)
 
void quaternionPosition3DFromPosition6D (const Position6D &pPos6D, Quaternion &pQua, Position3D &pPos3D)
 Convert a Position6D to Quaternion and Position3D More...
 
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. The inertia value is More...
 
std::ostream & operator<< (std::ostream &pStream, const Pose2D &pPos)
 Overloading of operator << for Pose2D. More...
 
std::ostream & operator<< (std::ostream &pStream, const Position2D &pPos)
 Overloading of operator << for Position2D. More...
 
std::ostream & operator<< (std::ostream &pStream, const Position3D &pPos)
 Overloading of operator << for Position3D. More...
 
std::ostream & operator<< (std::ostream &pStream, const Position6D &pPos)
 Overloading of operator << for Position6D. More...
 
std::ostream & operator<< (std::ostream &pStream, const PositionAndVelocity &pPosVel)
 Overloading of operator << for PositionAndVelocity. More...
 
std::ostream & operator<< (std::ostream &pStream, const Rotation &pRot)
 Overloading of operator << for Rotation. More...
 
std::ostream & operator<< (std::ostream &pStream, const Rotation3D &pRot)
 Overloading of operator << for Rotation3D. More...
 
std::ostream & operator<< (std::ostream &pStream, const Transform &pT)
 Overloading of operator << for Transform. More...
 
std::ostream & operator<< (std::ostream &pStream, const TransformAndVelocity6D &pTV)
 Overloading of operator << for TransformAndVelocity6D. More...
 
std::ostream & operator<< (std::ostream &pStream, const Velocity3D &pVel)
 Overloading of operator << for Velocity3D. More...
 
std::ostream & operator<< (std::ostream &pStream, const Quaternion &pQua)
 Overloading of operator << for Quaternion. More...
 
std::ostream & operator<< (std::ostream &pStream, const Velocity6D &pVel)
 Overloading of operator << for Velocity6D. More...
 
std::string toSpaceSeparated (const Position3D &pPos)
 Create a string of Position3D. More...
 
std::string toSpaceSeparated (const Rotation3D &pPos)
 Create a string of Rotation3D. More...
 
std::string toSpaceSeparated (const Velocity6D &pVel)
 Create a string of Velocity6D. More...
 
std::string toSpaceSeparated (const Transform &pT)
 Create a string of Transform. More...
 
std::string toSpaceSeparated (const Position6D &pPos)
 Create a string of Position6D. More...
 
std::string toSpaceSeparated (const Quaternion &pQuat)
 Create a string of Quaternion. More...
 
void transformLogarithmInPlace (const Transform &pT, Velocity6D &pVel)
 Compute the logarithme of a transform. Angle must be between $\left[-\pi+0.001, \pi-0.001\right]$ More...
 
Velocity6D transformLogarithm (const Transform &pT)
 Compute the logarithme of a transform. Angle must be between $\left[-\pi+0.001, \pi-0.001\right] $ More...
 
Transform velocityExponential (const Velocity6D &pVel)
 
void velocityExponentialInPlace (const Velocity6D &pVel, Transform &pT)
 
void changeReferenceVelocity6D (const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)
 
void changeReferencePosition6D (const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)
 
void changeReferencePosition3DInPlace (const Transform &pT, Position3D &pPosOut)
 
void changeReferenceTransposePosition3DInPlace (const Transform &pT, Position3D &pPosOut)
 
void changeReferencePosition3D (const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)
 
void changeReferenceTransposePosition3D (const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)
 
void changeReferenceTransform (const Transform &pT, const Transform &pTIn, Transform &pTOut)
 
void changeReferenceTransposeTransform (const Transform &pT, const Transform &pTIn, Transform &pTOut)
 
void changeReferenceTransposeVelocity6D (const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)
 
void changeReferenceTransposePosition6D (const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)
 
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. More...
 
Transform transformMean (const Transform &pTIn1, const Transform &pTIn2, const float &pVal=0.5f)
 Preform a logarithmic mean of pTIn1 and pTIn2. More...
 
Transform transformFromRotationPosition3D (const Rotation &pRot, const float &pX, const float &pY, const float &pZ)
 Create a Transform from 3D cartesian coordiantes and a Rotation. More...
 
Transform transformFromRotationPosition3D (const Rotation &pRot, const Position3D &pPos)
 Create a Transform from a Position3D and a Rotation. More...
 
void transformFromPosition3DInPlace (const Position3D &pPosition, Transform &pTransform)
 Modify pTransform to set the translation part to pPosition. More...
 
Transform transformFromPosition3D (const Position3D &pPosition)
 Create a 4*4 transform matrix from cartesian coordinates given in pPosition. More...
 
void transformFromRotationInPlace (const Rotation &pRotation, Transform &pTransform)
 Modify the rotation part of the transform. The translation part of the transform is not modified. More...
 
Transform transformFromRotation (const Rotation &pRotation)
 
void rotationFromTransformInPlace (const Transform &pTransform, Rotation &pRotation)
 Extract the position coordinates from a Transform. More...
 
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) * Ry(wy) * Rx(wx) = R More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
void position6DFromTransformInPlace (const Transform &pT, Position6D &pPos)
 Compute Position6D corresponding to the Transform. More...
 
Position6D position6DFromTransform (const Transform &pT)
 Compute Position6D corresponding to 4*4 Homogenous Transform. More...
 
void transformFromPose2DInPlace (const Pose2D &pPose, Transform &pT)
 Compute a Transform from a Pose2D. More...
 
Transform transformFromPose2D (const Pose2D &pPose)
 Create a Transform from a Pose2D. More...
 
void pose2DFromTransformInPlace (const Transform &pT, Pose2D &pPos)
 Compute a Pose2D from a Transform. More...
 
Pose2D pose2DFromTransform (const Transform &pT)
 Create a Pose2D from a Transform. More...
 
void position2DFromTransformInPlace (const Transform &pT, Position2D &pPos)
 Compute a Position2D from a Transform. More...
 
Position2D position2DFromTransform (const Transform &pT)
 Create a Position2D from a Transform. More...
 
Transform transformFromRotation3D (const Rotation3D &pRotation)
 Create a Transform from the 3 angles stored in a Rotation3D. The angles are composed in the following order: Rz(wz) * Ry(wy) * Rx(wx) = R More...
 
Transform transformFromPosition6D (const Position6D &pPosition6D)
 Create a Transform from a Position6D. More...
 
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 4*4 Homogenous transform matrix target. More...
 
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 4*4 Homogenous transform matrix target. More...
 
void position3DFromTransformInPlace (const Transform &pT, Position3D &pPos)
 Compute a Position3D from a Transform. More...
 
Position3D position3DFromTransform (const Transform &pT)
 Create a Position3D from a Transform. More...
 
Rotation3D rotation3DFromTransform (const Transform &pT)
 Create a Rotation3D (Roll, Pitch, Yaw) corresponding to the rotational part of the Transform. R = Rz(wz) * Ry(wy) * Rx(wx) More...
 
void transformFromRotVecInPlace (const int pAxis, const float pTheta, const Position3D &pPos, Transform &pT)
 Compute a Transform from More...
 
Transform transformFromRotVec (const int pAxis, const float pTheta, const Position3D &pPos)
 
void transformFromRotVecInPlace (const Position3D &pPos, Transform &pT)
 
Transform transformFromRotVec (const Position3D &pPos)
 
Transform transformFromRotVec (const int &pAxis, const float &pTheta)
 
Position3D operator* (const Transform &pT, const Position2D &pPos)
 
Position3D operator* (const Transform &pT, const Position3D &pPos)
 
Transform axisRotationProjection (const Position3D &pAxis, const Transform &pT)
 
Rotation axisRotationProjection (const Position3D &pAxis, const Rotation &pRot)
 
void axisRotationProjectionInPlace (const Position3D &pAxis, Transform &pT)
 
void axisRotationProjectionInPlace (const Position3D &pAxis, Rotation &pRot)
 
void orthogonalSpace (const Position3D &pPos, Transform &pTf)
 
Transform orthogonalSpace (const Position3D &pPos)
 
Transform transformFromQuaternion (const Quaternion &pQua)
 
Quaternion quaternionFromTransform (const Transform &pT)
 
Transform transformFromDisplacement (const Displacement &pDisp)
 
Displacement displacementFromTransform (const Transform &pTrans)
 
const bool avoidFootCollision (const std::vector< Position2D > &pLFootBoundingBox, const std::vector< Position2D > &pRFootBoundingBox, const bool &pIsLeftSupport, Pose2D &pMove)
 Compute the best position(orientation) of the foot to avoid collision. More...
 
const bool clipFootWithEllipse (const float &pMaxFootX, const float &pMaxFootY, Pose2D &pMove)
 Clip foot move with ellipsoid function More...
 
const bool areTwoBoxesInCollision (const std::vector< Position2D > &pBoxA, const std::vector< Position2D > &pBoxB)
 
void computeBox (const std::vector< Position2D > &pInitBox, const Pose2D &pMove, std::vector< Position2D > &pMovedBox)
 
const void dichotomie (const std::vector< Position2D > &pFixedBox, const std::vector< Position2D > &pMovingBox, Pose2D &pMove)
 
bool intersectionSegment2D (const Position2D &pA1, const Position2D &pA2, const Position2D &pB1, const Position2D &pB2, Position2D &pC)
 
bool isAxisMask (const int pAxisMask)
 Validates an AxisMask. More...
 
float distanceSquared (const Pose2D &pPos1, const Pose2D &pPos2)
 Compute the squared distance between two Pose2D. More...
 
float distance (const Pose2D &pPos1, const Pose2D &pPos2)
 Compute the distance between two Pose2D. More...
 
void pose2dInvertInPlace (Pose2D &pPos)
 Inverse the given Pose2D in place: More...
 
Pose2D pinv (const Pose2D &pPos)
 Alternative name for inverse: return the pose2d inverse of the given Pose2D. More...
 
Pose2D pose2dDiff (const Pose2D &pPos1, const Pose2D &pPos2)
 Compute the Pose2D between the actual Pose2D and the one give in argument result: More...
 
Pose2D pose2DInverse (const Pose2D &pPos)
 Compute the inverse of a Pose2D. More...
 
void pose2DInverse (const Pose2D &pPos, Pose2D &pRes)
 Compute the inverse of a Pose2D. More...
 
Position2D operator* (const float pM, const Position2D &pPos1)
 
float distanceSquared (const Position2D &pPos1, const Position2D &pPos2)
 Compute the squared distance between two Position2D. More...
 
float distance (const Position2D &pPos1, const Position2D &pPos2)
 Compute the distance between two Position2D $(pPos1,pPos2)$: More...
 
float norm (const Position2D &pPos)
 Compute the norm of a Position2D. More...
 
Position2D normalize (const Position2D &pPos)
 Normalize a Position2D. More...
 
float dotProduct (const Position2D &pPos1, const Position2D &pPos2)
 Compute the dot Product between two Position2D: More...
 
float crossProduct (const Position2D &pPos1, const Position2D &pPos2)
 Compute the cross Product of two Position2D. More...
 
void crossProduct (const Position2D &pPos1, const Position2D &pPos2, float &pRes)
 Compute the cross Product of two Position2D. More...
 
float distanceSquared (const Position3D &pPos1, const Position3D &pPos2)
 Compute the squared distance between two Position3D: More...
 
float distance (const Position3D &pPos1, const Position3D &pPos2)
 Compute the distance between two Position3D: More...
 
float norm (const Position3D &pPos)
 Compute the norm of a Position3D: More...
 
Position3D normalize (const Position3D &pPos)
 Normalize a Position3D: More...
 
float dotProduct (const Position3D &pPos1, const Position3D &pPos2)
 Compute the dot Product between two Position3D: More...
 
Position3D crossProduct (const Position3D &pPos1, const Position3D &pPos2)
 Compute the cross Product between two Position3D: More...
 
void crossProduct (const Position3D &pPos1, const Position3D &pPos2, Position3D &pRes)
 Compute the cross Product between two Position3D: More...
 
bool isUnitVector (const Position3D &pPos, const float &pEpsilon=0.0001f)
 Checks if the norm of a Position3D is near to 1.0 More...
 
bool isOrthogonal (const Position3D &pPos1, const Position3D &pPos2, const float &pEpsilon=0.0001f)
 Checks if two Position3D are orthogonal More...
 
float distanceSquared (const Position6D &pPos1, const Position6D &pPos2)
 Compute the squared distance of translation part (x, y and z) between two Position6D: More...
 
float distance (const Position6D &pPos1, const Position6D &pPos2)
 Compute the distance of translation part (x, y and z) between two Position6D: More...
 
float norm (const Position6D &pPos)
 Compute the norm of a Position6D: More...
 
Position6D normalize (const Position6D &pPos)
 Normalize a Position6D: More...
 
float norm (const Quaternion &pQua)
 Compute the norm of a Quaternion: More...
 
Quaternion normalize (const Quaternion &pQua)
 Normalize a Quaternion: More...
 
void quaternionInverse (const Quaternion &pQua, Quaternion &pQuaOut)
 Return the quaternion inverse of the given Quaternion: More...
 
Quaternion quaternionInverse (const Quaternion &pQua)
 Return the quaternion inverse of the given Quaternion More...
 
Quaternion quaternionFromAngleAndAxisRotation (const float pAngle, const float pAxisX, const float pAxisY, const float pAxisZ)
 Create a Quaternion initialized with explicit angle and axis rotation. More...
 
void angleAndAxisRotationFromQuaternion (const Quaternion &pQuaternion, float &pAngle, float &pAxisX, float &pAxisY, float &pAxisZ)
 Compute angle and axis rotation from a Quaternion. More...
 
std::vector< float > angleAndAxisRotationFromQuaternion (const Quaternion &pQuaternion)
 Compute angle and axis rotation from a Quaternion. More...
 
Rotation transpose (const Rotation &pRot)
 Compute the transpose rotation of the one given in argument: More...
 
float determinant (const Rotation &pRot)
 Compute the determinant of the given Rotation: More...
 
void normalizeRotation (Rotation &pRot)
 Normalize data, if needed, to have Rotation properties. More...
 
Rotation rotationFromQuaternion (const float pA, const float pB, const float pC, const float pD)
 Creates a 3*3 Rotation Matrix from a normalized quaternion ( |a + bi + cj + dk| = 1). More...
 
Rotation rotationFromAngleDirection (const float pAngle, const float pX, const float pY, const float pZ)
 Creates a 3*3 Rotation Matrix from a an angle and a normalized direction( |pX, pY, pZ| = 1). More...
 
void applyRotation (const AL::Math::Rotation &pRot, float &pX, float &pY, float &pZ)
 Apply Rotation to a 3D point. More...
 
Rotation rotationFromRotX (const float pRotX)
 Create a Rotation initialized with explicit rotation around x axis. More...
 
Rotation rotationFromRotY (const float pRotY)
 Create a Rotation initialized with explicit rotation around y axis. More...
 
Rotation rotationFromRotZ (const float pRotZ)
 Create a Rotation initialized with explicit rotation around z axis. More...
 
Rotation rotationFrom3DRotation (const float &pWX, const float &pWY, const float &pWZ)
 Create a Rotation initialized with euler angle. Rot = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX) More...
 
float norm (const Rotation3D &pRot)
 Compute the norm of a Rotation3D: More...
 
void transformPreMultiply (const Transform &pT, Transform &pTOut)
 pTOut = pT*pTOut More...
 
float norm (const Transform &pT)
 Compute the norm translation part of the actual Transform: More...
 
void normalizeTransform (Transform &pT)
 Normalize data, if needed, to have transform properties. More...
 
void transformToFloatVector (const Transform &pT, std::vector< float > &pTOut)
 DEPRECATED: Use toVector function. Copy the Transform in a vector of float: More...
 
std::vector< float > transformToFloatVector (const Transform &pT)
 DEPRECATED: Use toVector function. Return the Transform in a vector of float: More...
 
float determinant (const Transform &pT)
 Compute the determinant of rotation part of the given Transform: More...
 
float determinant (const std::vector< float > &pFloats)
 Compute the determinant of rotation part of the given vector of floats: More...
 
void transformInverse (const Transform &pT, Transform &pTOut)
 Return the transform inverse of the given Transform: More...
 
Transform transformInverse (const Transform &pT)
 Return the transform inverse of the given Transform: More...
 
Transform transformFromRotX (const float pRotX)
 Create a Transform initialize with explicit rotation around x axis: More...
 
Transform transformFromRotY (const float pRotY)
 Create a Transform initialize with explicit rotation around y axis: More...
 
Transform transformFromRotZ (const float pRotZ)
 Create a Transform initialize with explicit rotation around z axis: More...
 
Transform transformFrom3DRotation (const float &pWX, const float &pWY, const float &pWZ)
 Create a Transform initialize with euler angle: H = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX) More...
 
Transform transformFromPosition (const float &pX, const float &pY, const float &pZ)
 Create a Transform initialize with explicit value for translation part: More...
 
Transform transformFromPosition (const float &pX, const float &pY, const float &pZ, const float &pWX, const float &pWY, const float &pWZ)
 Create a Transform initialize with explicit value for translation part and euler angle: More...
 
void transformInvertInPlace (Transform &pT)
 Inverse the given Transform in place: More...
 
Transform pinv (const Transform &pT)
 Alternative name for inverse: return the transform inverse of the given Transform: More...
 
Transform transformDiff (const Transform &pT1, const Transform &pT2)
 Compute the Transform between the actual Transform and the one give in argument result: More...
 
float transformDistanceSquared (const Transform &pT1, const Transform &pT2)
 Compute the squared distance between the actual Transform and the one give in argument (translation part): More...
 
float transformDistance (const Transform &pT1, const Transform &pT2)
 Compute the distance between the actual Transform and the one give in argument: More...
 
Velocity3D operator* (const float pM, const Velocity3D &pVel1)
 
float norm (const Velocity3D &pVel)
 Compute the norm of a Velocity3D: More...
 
Velocity3D normalize (const Velocity3D &pVel)
 Normalize a Velocity3D: More...
 
Velocity6D operator* (const float pVal, const Velocity6D &pVel)
 Overloading of operator * for left Velocity6D. More...
 
float norm (const Velocity6D &pVel)
 Compute the norm of a Velocity6D: More...
 
Velocity6D normalize (const Velocity6D &pVel)
 Normalize a Velocity6D: More...
 

Typedef Documentation

typedef Eigen::Matrix<float, 3, 4> AL::Math::Matrix34f

Definition at line 19 of file almatheigen.h.

typedef Eigen::Matrix<float, 3, 4, Eigen::RowMajor> AL::Math::Matrix34frm

Definition at line 20 of file almatheigen.h.

typedef Eigen::Matrix<float, 6, 1> AL::Math::Vector6f

Definition at line 21 of file almatheigen.h.

Function Documentation

template<typename Scalar , typename Derived0 , typename Derived1 >
void AL::Math::addPointMassInertia ( Scalar  mass,
const Eigen::MatrixBase< Derived0 > &  position,
Eigen::MatrixBase< Derived1 > &  output 
)

Definition at line 47 of file urdfeigen.h.

const bool AL::Math::areTwoBoxesInCollision ( const std::vector< Position2D > &  pBoxA,
const std::vector< Position2D > &  pBoxB 
)
Transform AL::Math::axisRotationProjection ( const Position3D &  pAxis,
const Transform &  pT 
)

Function axisRotationProjection: finding the closest rotation Rw of R around an axis (Position3D)

Parameters
pAxisaxis of rotation
pTa transform
Returns
Transform
Rotation AL::Math::axisRotationProjection ( const Position3D &  pAxis,
const Rotation &  pRot 
)

Function axisRotationProjection: finding the closest rotation Rw of R around an axis (Position3D)

Parameters
pAxisaxis of rotation
pRota rotation
Returns
Rotation
void AL::Math::axisRotationProjectionInPlace ( const Position3D &  pAxis,
Transform &  pT 
)

Function axisRotationProjectionInPlace: finding the closest rotation Rw of R around an axis (Position3D)

Parameters
pAxisaxis of rotation
pTa transform
void AL::Math::axisRotationProjectionInPlace ( const Position3D &  pAxis,
Rotation &  pRot 
)

Function axisRotationProjectionInPlace: finding the closest rotation Rw of R around an axis (Position3D)

Parameters
pAxisaxis of rotation
pRota rotation
void AL::Math::changeReferencePosition3DInPlace ( const Transform &  pT,
Position3D &  pPosOut 
)
void AL::Math::changeReferenceTransposePosition3DInPlace ( const Transform &  pT,
Position3D &  pPosOut 
)
bool AL::Math::clipData ( const float &  pMin,
const float &  pMax,
std::vector< float > &  pData 
)
bool AL::Math::clipData ( const float &  pMin,
const float &  pMax,
std::vector< std::vector< float > > &  pData 
)
void AL::Math::computeBox ( const std::vector< Position2D > &  pInitBox,
const Pose2D &  pMove,
std::vector< Position2D > &  pMovedBox 
)
void AL::Math::computePillParameters ( const Math::Position3D &  pEndTfToCenterA,
const Math::Position3D &  pEndTfToCenterB,
float &  pHalfExtent,
Math::Transform &  pEndTfToPillCenter 
)
Position3D AL::Math::crossProduct ( const Position3D &  pPos1,
const Position3D &  pPos2 
)

Compute the cross Product between two Position3D:

$pRes.x = pPos1.y*pPos2.z - pPos1.z*pPos2.y$
$pRes.y = pPos1.z*pPos2.x - pPos1.x*pPos2.z$
$pRes.z = pPos1.x*pPos2.y - pPos1.y*pPos2.x$

Parameters
pPos1the first Position3D
pPos2the second Position3D
Returns
the Position3D cross product between the two Position3D
void AL::Math::crossProduct ( const Position3D &  pPos1,
const Position3D &  pPos2,
Position3D &  pRes 
)

Compute the cross Product between two Position3D:

$pRes.x = pPos1.y*pPos2.z - pPos1.z*pPos2.y$
$pRes.y = pPos1.z*pPos2.x - pPos1.x*pPos2.z$
$pRes.z = pPos1.x*pPos2.y - pPos1.y*pPos2.x$

Parameters
pPos1the first Position3D
pPos2the second Position3D
pResthe Position3D cross product between the two Position3D
const void AL::Math::dichotomie ( const std::vector< Position2D > &  pFixedBox,
const std::vector< Position2D > &  pMovingBox,
Pose2D &  pMove 
)
Displacement AL::Math::displacementFromTransform ( const Transform &  pTrans)

Function displacementFromTransform: computing the displacement equivalent to the transform

Parameters
pTransa transform
Returns
computed Displacement
float AL::Math::dotProduct ( const Position2D &  pPos1,
const Position2D &  pPos2 
)

Compute the dot Product between two Position2D:

$pRes = (pPos1.x*pPos2.x + pPos1.y*pPos2.y)$

Parameters
pPos1the first Position2D
pPos2the second Position2D
Returns
the float dot product between the two Position2D
float AL::Math::dotProduct ( const Position3D &  pPos1,
const Position3D &  pPos2 
)

Compute the dot Product between two Position3D:

$pRes = (pPos1.x*pPos2.x + pPos1.y*pPos2.y + pPos1.z*pPos2.z)$

Parameters
pPos1the first Position3D
pPos2the second Position3D
Returns
the float dot product between the two Position3D
Eigen::Quaterniond AL::Math::eigenQuaternionFromUrdfRpy ( const AL::urdf::Array3d rpy)
inline

Definition at line 15 of file urdfeigen.h.

void AL::Math::getSphereCentersFromPill ( float  pHalfExtent,
const Math::Transform &  pEndTfToPillCenter,
Math::Position3D &  pCenterA,
Math::Position3D &  pCenterB 
)
bool AL::Math::intersectionSegment2D ( const Position2D &  pA1,
const Position2D &  pA2,
const Position2D &  pB1,
const Position2D &  pB2,
Position2D &  pC 
)
bool AL::Math::isAxisMask ( const int  pAxisMask)

Validates an AxisMask.

Parameters
pAxisMaskAn Axis Mask.
Returns
true if it succeeds, false if it fails.
Velocity3D AL::Math::operator* ( const float  pM,
const Velocity3D &  pVel1 
)
Velocity6D AL::Math::operator* ( const float  pVal,
const Velocity6D &  pVel 
)

Overloading of operator * for left Velocity6D.

Parameters
pValthe float factor.
pVelthe given Velocity6D.
Position2D AL::Math::operator* ( const float  pM,
const Position2D &  pPos1 
)
Position3D AL::Math::operator* ( const Transform &  pT,
const Position2D &  pPos 
)
Position3D AL::Math::operator* ( const Transform &  pT,
const Position3D &  pPos 
)
void AL::Math::orthogonalSpace ( const Position3D &  pPos,
Transform &  pTf 
)

Function orthogonalSpace: create an orthonormal direct base from a vector (Position3D)

Parameters
pPosa vector of direction
pTfthe result transform
Transform AL::Math::orthogonalSpace ( const Position3D &  pPos)
Quaternion AL::Math::quaternionFromRotation3D ( const Rotation3D &  pRot3D)
Quaternion AL::Math::quaternionFromTransform ( const Transform &  pT)
Rotation3D AL::Math::rotation3DFromQuaternion ( const Quaternion &  pQuaternion)
Rotation AL::Math::rotationFromQuaternion ( const Quaternion &  pQua)
template<typename Scalar , typename EPose , typename Derived1 >
void AL::Math::squashInertial ( Scalar  mass_a,
const EPose &  pose_a,
const Eigen::MatrixBase< Derived1 > &  inertia_a,
Scalar  mass_b,
const EPose &  pose_b,
const Eigen::MatrixBase< Derived1 > &  inertia_b,
Scalar &  mass_c,
EPose &  pose_c,
Eigen::MatrixBase< Derived1 > &  inertia_c 
)

Definition at line 81 of file urdfeigen.h.

template<typename Derived0 >
Position3D AL::Math::toALMathPosition3D ( const Eigen::MatrixBase< Derived0 > &  in)

Definition at line 72 of file almatheigen.h.

template<typename Derived0 >
void AL::Math::toALMathTransform ( const Eigen::MatrixBase< Derived0 > &  in,
Transform &  out 
)

Definition at line 42 of file almatheigen.h.

template<typename Derived0 >
Transform AL::Math::toALMathTransform ( const Eigen::MatrixBase< Derived0 > &  m)

Definition at line 51 of file almatheigen.h.

template<int _Mode, int _Options>
void AL::Math::toALMathTransform ( const Eigen::Transform< float, 3, _Mode, _Options > &  in,
Transform &  out 
)

Definition at line 58 of file almatheigen.h.

template<int _Mode, int _Options>
Transform AL::Math::toALMathTransform ( const Eigen::Transform< float, 3, _Mode, _Options > &  tr)

Definition at line 64 of file almatheigen.h.

template<typename Derived0 >
void AL::Math::toALMathVelocity6D ( const Eigen::MatrixBase< Derived0 > &  in,
Velocity6D &  out 
)

Definition at line 81 of file almatheigen.h.

template<typename Derived0 >
Velocity6D AL::Math::toALMathVelocity6D ( const Eigen::MatrixBase< Derived0 > &  in)

Definition at line 92 of file almatheigen.h.

Eigen::AffineCompact3f AL::Math::toEigenAffineCompact3 ( const AL::Math::Transform tr)
inline

Definition at line 36 of file almatheigen.h.

Eigen::Matrix3f AL::Math::toEigenMatrix3 ( const AL::Math::Transform tr)
inline

Definition at line 27 of file almatheigen.h.

Eigen::Matrix3d AL::Math::toEigenMatrix3 ( const AL::urdf::Inertial inertial)
inline

Definition at line 34 of file urdfeigen.h.

Matrix34f AL::Math::toEigenMatrix34 ( const AL::Math::Transform tr)
inline

Definition at line 23 of file almatheigen.h.

Eigen::Isometry3d AL::Math::toEigenTransform ( const AL::urdf::Pose p)
inline

Definition at line 28 of file urdfeigen.h.

Eigen::Vector3f AL::Math::toEigenVector3 ( const AL::Math::Position3D v)
inline

Definition at line 32 of file almatheigen.h.

qi::Duration AL::Math::toQiDuration ( ros::Time  time)
inline

Definition at line 64 of file qirosmsg.h.

ros::Time AL::Math::toValidRosTime ( qi::Duration  time_since_epoch)
inline

Definition at line 35 of file qirosmsg.h.

Transform AL::Math::transformFromDisplacement ( const Displacement &  pDisp)

Function transformFromDisplacement: computing the transform equivalent to the displacement

Parameters
pDispa displacement
Returns
computed Transform
Transform AL::Math::transformFromQuaternion ( const Quaternion &  pQua)
template<typename Scalar , typename Derived0 , typename Derived1 >
void AL::Math::urdfInertialToEigen ( const AL::urdf::Inertial inertial,
Scalar &  mass,
Eigen::MatrixBase< Derived0 > &  com_a,
Eigen::MatrixBase< Derived1 > &  inertia_a 
)

Definition at line 119 of file urdfeigen.h.

template<typename T >
AL::urdf::Array3d AL::Math::urdfRpyFromEigenMatrix3 ( const T &  t)

Definition at line 23 of file urdfeigen.h.

void AL::Math::velocityExponentialInPlace ( const Velocity6D &  pVel,
Transform &  pT 
)