libalmath  2.5.11.14a
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Classes | Typedefs | Functions
Types

Classes

struct  AL::Math::Pose2D
 A pose in a 2-dimentional space. More...
 
struct  AL::Math::Position2D
 Create and play with a Position2D. More...
 
struct  AL::Math::Position3D
 Create and play with a Position3D. More...
 
struct  AL::Math::Position6D
 Create and play with a Position6D. More...
 
struct  AL::Math::PositionAndVelocity
 Create and play with a PositionAndVelocity. More...
 
struct  AL::Math::Quaternion
 Create and play with a Quaternion. More...
 
struct  AL::Math::Rotation
 A 3*3 rotation matrix. More...
 
struct  AL::Math::Rotation3D
 A Rotation3D give 3 composed angles in radians. More...
 
struct  AL::Math::Transform
 A homogenous transformation matrix. More...
 
struct  AL::Math::Velocity3D
 Create and play with a Velocity3D. More...
 
struct  AL::Math::Velocity6D
 Create and play with a Velocity6D. More...
 

Typedefs

typedef std::bitset< 6 > AL::Math::AXIS_MASK
 Definition of an AXIS_MASK as a bit set. More...
 

Functions

std::ostream & AL::Math::operator<< (std::ostream &pStream, const Pose2D &pPos)
 Overloading of operator << for Pose2D. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Position2D &pPos)
 Overloading of operator << for Position2D. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Position3D &pPos)
 Overloading of operator << for Position3D. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Position6D &pPos)
 Overloading of operator << for Position6D. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const PositionAndVelocity &pPosVel)
 Overloading of operator << for PositionAndVelocity. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Rotation &pRot)
 Overloading of operator << for Rotation. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Rotation3D &pRot)
 Overloading of operator << for Rotation3D. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Transform &pT)
 Overloading of operator << for Transform. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const TransformAndVelocity6D &pTV)
 Overloading of operator << for TransformAndVelocity6D. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Velocity3D &pVel)
 Overloading of operator << for Velocity3D. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Quaternion &pQua)
 Overloading of operator << for Quaternion. More...
 
std::ostream & AL::Math::operator<< (std::ostream &pStream, const Velocity6D &pVel)
 Overloading of operator << for Velocity6D. More...
 
std::string AL::Math::toSpaceSeparated (const Position3D &pPos)
 Create a string of Position3D. More...
 
std::string AL::Math::toSpaceSeparated (const Rotation3D &pPos)
 Create a string of Rotation3D. More...
 
std::string AL::Math::toSpaceSeparated (const Velocity6D &pVel)
 Create a string of Velocity6D. More...
 
std::string AL::Math::toSpaceSeparated (const Transform &pT)
 Create a string of Transform. More...
 
std::string AL::Math::toSpaceSeparated (const Position6D &pPos)
 Create a string of Position6D. More...
 
std::string AL::Math::toSpaceSeparated (const Quaternion &pQuat)
 Create a string of Quaternion. More...
 
float AL::Math::distanceSquared (const Pose2D &pPos1, const Pose2D &pPos2)
 Compute the squared distance between two Pose2D. More...
 
float AL::Math::distance (const Pose2D &pPos1, const Pose2D &pPos2)
 Compute the distance between two Pose2D. More...
 
void AL::Math::pose2dInvertInPlace (Pose2D &pPos)
 Inverse the given Pose2D in place: More...
 
Pose2D AL::Math::pinv (const Pose2D &pPos)
 Alternative name for inverse: return the pose2d inverse of the given Pose2D. More...
 
Pose2D AL::Math::pose2dDiff (const Pose2D &pPos1, const Pose2D &pPos2)
 Compute the Pose2D between the actual Pose2D and the one give in argument result: More...
 
Pose2D AL::Math::pose2DInverse (const Pose2D &pPos)
 Compute the inverse of a Pose2D. More...
 
void AL::Math::pose2DInverse (const Pose2D &pPos, Pose2D &pRes)
 Compute the inverse of a Pose2D. More...
 
float AL::Math::distanceSquared (const Position2D &pPos1, const Position2D &pPos2)
 Compute the squared distance between two Position2D. More...
 
float AL::Math::distance (const Position2D &pPos1, const Position2D &pPos2)
 Compute the distance between two Position2D $(pPos1,pPos2)$: More...
 
float AL::Math::norm (const Position2D &pPos)
 Compute the norm of a Position2D. More...
 
Position2D AL::Math::normalize (const Position2D &pPos)
 Normalize a Position2D. More...
 
float AL::Math::crossProduct (const Position2D &pPos1, const Position2D &pPos2)
 Compute the cross Product of two Position2D. More...
 
void AL::Math::crossProduct (const Position2D &pPos1, const Position2D &pPos2, float &pRes)
 Compute the cross Product of two Position2D. More...
 
bool AL::Math::Position3D::isUnitVector (const float &pEpsilon=0.0001f) const
 Checks if the norm of the Position3D is near to 1.0 More...
 
bool AL::Math::Position3D::isOrthogonal (const Position3D &pPos, const float &pEpsilon=0.0001f) const
 Checks if this Position3D is orthogonal to Position3D pPos More...
 
float AL::Math::distanceSquared (const Position3D &pPos1, const Position3D &pPos2)
 Compute the squared distance between two Position3D: More...
 
float AL::Math::distance (const Position3D &pPos1, const Position3D &pPos2)
 Compute the distance between two Position3D: More...
 
float AL::Math::norm (const Position3D &pPos)
 Compute the norm of a Position3D: More...
 
Position3D AL::Math::normalize (const Position3D &pPos)
 Normalize a Position3D: More...
 
bool AL::Math::isUnitVector (const Position3D &pPos, const float &pEpsilon=0.0001f)
 Checks if the norm of a Position3D is near to 1.0 More...
 
bool AL::Math::isOrthogonal (const Position3D &pPos1, const Position3D &pPos2, const float &pEpsilon=0.0001f)
 Checks if two Position3D are orthogonal More...
 
float AL::Math::distanceSquared (const Position6D &pPos1, const Position6D &pPos2)
 Compute the squared distance of translation part (x, y and z) between two Position6D: More...
 
float AL::Math::distance (const Position6D &pPos1, const Position6D &pPos2)
 Compute the distance of translation part (x, y and z) between two Position6D: More...
 
float AL::Math::norm (const Position6D &pPos)
 Compute the norm of a Position6D: More...
 
Position6D AL::Math::normalize (const Position6D &pPos)
 Normalize a Position6D: More...
 
float AL::Math::norm (const Quaternion &pQua)
 Compute the norm of a Quaternion: More...
 
Quaternion AL::Math::normalize (const Quaternion &pQua)
 Normalize a Quaternion: More...
 
void AL::Math::quaternionInverse (const Quaternion &pQua, Quaternion &pQuaOut)
 Return the quaternion inverse of the given Quaternion: More...
 
Quaternion AL::Math::quaternionInverse (const Quaternion &pQua)
 Return the quaternion inverse of the given Quaternion More...
 
Quaternion AL::Math::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 AL::Math::angleAndAxisRotationFromQuaternion (const Quaternion &pQuaternion, float &pAngle, float &pAxisX, float &pAxisY, float &pAxisZ)
 Compute angle and axis rotation from a Quaternion. More...
 
std::vector< float > AL::Math::angleAndAxisRotationFromQuaternion (const Quaternion &pQuaternion)
 Compute angle and axis rotation from a Quaternion. More...
 
Rotation AL::Math::transpose (const Rotation &pRot)
 Compute the transpose rotation of the one given in argument: More...
 
float AL::Math::determinant (const Rotation &pRot)
 Compute the determinant of the given Rotation: More...
 
void AL::Math::normalizeRotation (Rotation &pRot)
 Normalize data, if needed, to have Rotation properties. More...
 
Rotation AL::Math::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 AL::Math::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 AL::Math::applyRotation (const AL::Math::Rotation &pRot, float &pX, float &pY, float &pZ)
 Apply Rotation to a 3D point. More...
 
Rotation AL::Math::rotationFromRotX (const float pRotX)
 Create a Rotation initialized with explicit rotation around x axis. More...
 
Rotation AL::Math::rotationFromRotY (const float pRotY)
 Create a Rotation initialized with explicit rotation around y axis. More...
 
Rotation AL::Math::rotationFromRotZ (const float pRotZ)
 Create a Rotation initialized with explicit rotation around z axis. More...
 
Rotation AL::Math::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 AL::Math::norm (const Rotation3D &pRot)
 Compute the norm of a Rotation3D: More...
 
void AL::Math::transformPreMultiply (const Transform &pT, Transform &pTOut)
 pTOut = pT*pTOut More...
 
float AL::Math::norm (const Transform &pT)
 Compute the norm translation part of the actual Transform: More...
 
void AL::Math::normalizeTransform (Transform &pT)
 Normalize data, if needed, to have transform properties. More...
 
void AL::Math::transformToFloatVector (const Transform &pT, std::vector< float > &pTOut)
 DEPRECATED: Use toVector function. Copy the Transform in a vector of float: More...
 
std::vector< float > AL::Math::transformToFloatVector (const Transform &pT)
 DEPRECATED: Use toVector function. Return the Transform in a vector of float: More...
 
float AL::Math::determinant (const Transform &pT)
 Compute the determinant of rotation part of the given Transform: More...
 
float AL::Math::determinant (const std::vector< float > &pFloats)
 Compute the determinant of rotation part of the given vector of floats: More...
 
void AL::Math::transformInverse (const Transform &pT, Transform &pTOut)
 Return the transform inverse of the given Transform: More...
 
Transform AL::Math::transformInverse (const Transform &pT)
 Return the transform inverse of the given Transform: More...
 
Transform AL::Math::transformFromRotX (const float pRotX)
 Create a Transform initialize with explicit rotation around x axis: More...
 
Transform AL::Math::transformFromRotY (const float pRotY)
 Create a Transform initialize with explicit rotation around y axis: More...
 
Transform AL::Math::transformFromRotZ (const float pRotZ)
 Create a Transform initialize with explicit rotation around z axis: More...
 
Transform AL::Math::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 AL::Math::transformFromPosition (const float &pX, const float &pY, const float &pZ)
 Create a Transform initialize with explicit value for translation part: More...
 
Transform AL::Math::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 AL::Math::transformInvertInPlace (Transform &pT)
 Inverse the given Transform in place: More...
 
Transform AL::Math::pinv (const Transform &pT)
 Alternative name for inverse: return the transform inverse of the given Transform: More...
 
Transform AL::Math::transformDiff (const Transform &pT1, const Transform &pT2)
 Compute the Transform between the actual Transform and the one give in argument result: More...
 
float AL::Math::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 AL::Math::transformDistance (const Transform &pT1, const Transform &pT2)
 Compute the distance between the actual Transform and the one give in argument: More...
 
float AL::Math::norm (const Velocity3D &pVel)
 Compute the norm of a Velocity3D: More...
 
Velocity3D AL::Math::normalize (const Velocity3D &pVel)
 Normalize a Velocity3D: More...
 
float AL::Math::norm (const Velocity6D &pVel)
 Compute the norm of a Velocity6D: More...
 
Velocity6D AL::Math::normalize (const Velocity6D &pVel)
 Normalize a Velocity6D: More...
 

Detailed Description

Types classes provide many usefull defintion in robotics field such as homogenous transform matrix, 6D vector of velocty and so on.

Typedef Documentation

typedef std::bitset<6> AL::Math::AXIS_MASK

Definition of an AXIS_MASK as a bit set.

static const int AXIS_MASK_X = 1
static const int AXIS_MASK_Y = 2
static const int AXIS_MASK_XY = 3
static const int AXIS_MASK_Z = 4
static const int AXIS_MASK_WX = 8
static const int AXIS_MASK_WY = 16
static const int AXIS_MASK_WZ = 32
static const int AXIS_MASK_WYWZ = 48
static const int AXIS_MASK_ALL = 63
static const int AXIS_MASK_VEL = 7
static const int AXIS_MASK_ROT = 56
static const int AXIS_MASK_NONE = 0
static const int AXIS_MASK_PLANEZ = 28

Definition at line 35 of file alaxismask.h.

Function Documentation

void AL::Math::angleAndAxisRotationFromQuaternion ( const Quaternion &  pQuaternion,
float &  pAngle,
float &  pAxisX,
float &  pAxisY,
float &  pAxisZ 
)

Compute angle and axis rotation from a Quaternion.

Parameters
pQuaternionthe given quaternion
pAnglethe computed float value for angle in radian around axis rotation
pAxisXthe computed float value for x value of axis rotation
pAxisYthe computed float value for y value of axis rotation
pAxisZthe computed float value for z value of axis rotation
std::vector<float> AL::Math::angleAndAxisRotationFromQuaternion ( const Quaternion &  pQuaternion)

Compute angle and axis rotation from a Quaternion.

Parameters
pQuaternionthe given quaternion
Returns
a vector containing angle, axisX, axisY, axisZ.
void AL::Math::applyRotation ( const AL::Math::Rotation pRot,
float &  pX,
float &  pY,
float &  pZ 
)

Apply Rotation to a 3D point.

Parameters
pRotthe given rotation
pXthe X position of the 3D point after rotation
pYthe Y position of the 3D point after rotation
pZthe Z position of the 3D point after rotation
float AL::Math::crossProduct ( const Position2D &  pPos1,
const Position2D &  pPos2 
)

Compute the cross Product of two Position2D.

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

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

Compute the cross Product of two Position2D.

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

Parameters
pPos1the first Position2D
pPos2the second Position2D
pResthe float cross product between the two Position2D
float AL::Math::determinant ( const Rotation &  pRot)

Compute the determinant of the given Rotation:

$pRot.r_1c_1*pRot.r_2c_2*pRot.r_3c_3 + pRot.r_1c_2*pRot.r_2c_3*pRot.r_3c_1 + pRot.r_1c_3*pRot.r_2c_1*pRot.r_3c_2 - pRot.r_1c_1*pRot.r_2c_3*pRot.r_3c_2 - pRot.r_1c_2*pRot.r_2c_1*pRot.r_3c_3 - pRot.r_1c_3*pRot.r_2c_2*pRot.r_3c_1$

Parameters
pRotthe given Rotation
Returns
the float determinant of Rotation
float AL::Math::determinant ( const Transform &  pT)

Compute the determinant of rotation part of the given Transform:

$pT.r_1c_1*pT.r_2c_2*pT.r_3c_3 + pT.r_1c_2*pT.r_2c_3*pT.r_3c_1 + pT.r_1c_3*pT.r_2c_1 * pT.r_3c_2 - pT.r_1c_1*pT.r_2c_3*pT.r_3c_2 - pT.r_1c_2*pT.r_2c_1*pT.r_3c_3 - pT.r_1c_3*pT.r_2c_2*pT.r_3c_1$

Parameters
pTthe given Transform
Returns
the float determinant of rotation Transform part
float AL::Math::determinant ( const std::vector< float > &  pFloats)

Compute the determinant of rotation part of the given vector of floats:

$pT[0]*pT[5]*pT[10] + pT[1]*pT[6]*pT[8] + pT[2]*pT[4]*pT[9] - pT[0]*pT[6]*pT[9] - pT[1]*pT[4]*pT[10] - pT[2]*pT[5]*pT[8]$

Parameters
pFloatsthe given vector of floats
Returns
the float determinant of rotation Transform part
float AL::Math::distance ( const Position2D &  pPos1,
const Position2D &  pPos2 
)

Compute the distance between two Position2D $(pPos1,pPos2)$:

$\sqrt{(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2}$

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

Compute the distance between two Position3D:

$\sqrt{(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2+(pPos1.z-pPos2.z)^2}$

Parameters
pPos1the first Position3D
pPos2the second Position3D
Returns
the float distance between the two Position3D
float AL::Math::distance ( const Position6D &  pPos1,
const Position6D &  pPos2 
)

Compute the distance of translation part (x, y and z) between two Position6D:

$\sqrt{(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2+(pPos1.z-pPos2.z)^2}$

Parameters
pPos1the first Position6D
pPos2the second Position6D
Returns
the float distance between the two Position6D
float AL::Math::distance ( const Pose2D &  pPos1,
const Pose2D &  pPos2 
)

Compute the distance between two Pose2D.

$\sqrt{(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2}$

Parameters
pPos1the first Pose2D
pPos2the second Pose2D
Returns
the float distance between the two Pose2D
float AL::Math::distanceSquared ( const Position2D &  pPos1,
const Position2D &  pPos2 
)

Compute the squared distance between two Position2D.

$(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2$

Parameters
pPos1the first Position2D
pPos2the second Position2D
Returns
the float squared distance between the two Position2D
float AL::Math::distanceSquared ( const Position3D &  pPos1,
const Position3D &  pPos2 
)

Compute the squared distance between two Position3D:

$(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2+(pPos1.z-pPos2.z)^2$

Parameters
pPos1the first Position3D
pPos2the second Position3D
Returns
the float squared distance between the two Position3D
float AL::Math::distanceSquared ( const Position6D &  pPos1,
const Position6D &  pPos2 
)

Compute the squared distance of translation part (x, y and z) between two Position6D:

$(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2+(pPos1.z-pPos2.z)^2$

Parameters
pPos1the first Position6D
pPos2the second Position6D
Returns
the float squared distance between the two Position6D
float AL::Math::distanceSquared ( const Pose2D &  pPos1,
const Pose2D &  pPos2 
)

Compute the squared distance between two Pose2D.

$(pPos1.x-pPos2.x)^2+(pPos1.y-pPos2.y)^2$

Parameters
pPos1the first Pose2D
pPos2the second Pose2D
Returns
the float squared distance between the two Pose2D
bool AL::Math::Position3D::isOrthogonal ( const Position3D pPos,
const float &  pEpsilon = 0.0001f 
) const

Checks if this Position3D is orthogonal to Position3D pPos

Parameters
pEpsilonan optional epsilon distance
Returns
true if orthogonal
bool AL::Math::isOrthogonal ( const Position3D &  pPos1,
const Position3D &  pPos2,
const float &  pEpsilon = 0.0001f 
)

Checks if two Position3D are orthogonal

Parameters
pPos1the first Position3D
pPos2the second Position3D
pEpsilonan optional epsilon distance
Returns
true if orthogonal
bool AL::Math::Position3D::isUnitVector ( const float &  pEpsilon = 0.0001f) const

Checks if the norm of the Position3D is near to 1.0

Parameters
pEpsilonan optional epsilon distance
Returns
true if the norm of the vector is near to 1.0
bool AL::Math::isUnitVector ( const Position3D &  pPos,
const float &  pEpsilon = 0.0001f 
)

Checks if the norm of a Position3D is near to 1.0

Parameters
pPosthe Position3D
pEpsilonan optional epsilon distance
Returns
true if the norm of the vector is near to 1.0
float AL::Math::norm ( const Rotation3D &  pRot)

Compute the norm of a Rotation3D:

$\sqrt{pRot.wx^2 + pRot.wy^2 + pRot.wz^2}$

Parameters
pRotthe given Rotation3D
Returns
the float norm of the given Rotation3D
float AL::Math::norm ( const Quaternion &  pQua)

Compute the norm of a Quaternion:

$\sqrt{pQua.w^2+pQua.x^2+pQua.y^2+pQua.z^2}$

Parameters
pQuathe given Quaternion
Returns
the float norm of the given Quaternion
float AL::Math::norm ( const Velocity3D &  pVel)

Compute the norm of a Velocity3D:

$\sqrt{pVel.xd^2 + pVel.yd^2 + pVel.zd^2}$

Parameters
pVelthe given Velocity3D
Returns
the float norm of the given Velocity3D
float AL::Math::norm ( const Velocity6D &  pVel)

Compute the norm of a Velocity6D:

$\sqrt{pVel.xd^2 + pVel.yd^2 + pVel.zd^2 + pVel.wxd^2 + pVel.wyd^2 + pVel.wzd^2}$

Parameters
pVelthe given Velocity6D
Returns
the float norm of the given Velocity6D
float AL::Math::norm ( const Position2D &  pPos)

Compute the norm of a Position2D.

$\sqrt{(pPos.x)^2+(pPos.y)^2}$

Parameters
pPosthe given Position2D
Returns
the float norm of the given Position2D
float AL::Math::norm ( const Position3D &  pPos)

Compute the norm of a Position3D:

$\sqrt{pPos.x^2+pPos.y^2+pPos.z^2}$

Parameters
pPosthe given Position3D
Returns
the float norm of the given Position3D
float AL::Math::norm ( const Position6D &  pPos)

Compute the norm of a Position6D:

$\sqrt{pPos.x^2 + pPos.y^2 + pPos.z^2 + pPos.wx^2 + pPos.wy^2 + pPos.wz^2}$

Parameters
pPosthe given Position6D
Returns
the float norm of the given Position6D
float AL::Math::norm ( const Transform &  pT)

Compute the norm translation part of the actual Transform:

$\sqrt{pT.r_1c_4^2+pT.r_2c_4^2+pT.r_3c_4^2}$

Parameters
pTthe given Transform
Returns
the float norm of the given Transform
Quaternion AL::Math::normalize ( const Quaternion &  pQua)

Normalize a Quaternion:

$pRes = \frac{pQua}{norm(pQua)}$

Parameters
pQuathe given Quaternion
Returns
the given Quaternion normalized
Velocity3D AL::Math::normalize ( const Velocity3D &  pVel)

Normalize a Velocity3D:

$ pRes = \frac{pVel}{norm(pVel)} $

Parameters
pVelthe given Velocity3D
Returns
the given Velocity3D normalized
Velocity6D AL::Math::normalize ( const Velocity6D &  pVel)

Normalize a Velocity6D:

$pRes = \frac{pVel}{norm(pVel)} $

Parameters
pVelthe given Velocity6D
Returns
the given Velocity6D normalized
Position2D AL::Math::normalize ( const Position2D &  pPos)

Normalize a Position2D.

$pRes = \frac{pPos}{norm(pPos)}$

Parameters
pPosthe given Position2D
Returns
the given Position2D normalized
Position3D AL::Math::normalize ( const Position3D &  pPos)

Normalize a Position3D:

$pRes = \frac{pPos}{norm(pPos)}$

Parameters
pPosthe given Position3D
Returns
the given Position3D normalized
Position6D AL::Math::normalize ( const Position6D &  pPos)

Normalize a Position6D:

$pRes = \frac{pPos}{norm(pPos)} $

Parameters
pPosthe given Position6D
Returns
the given Position6D normalized
void AL::Math::normalizeRotation ( Rotation &  pRot)

Normalize data, if needed, to have Rotation properties.

Parameters
pRotthe given Rotation
void AL::Math::normalizeTransform ( Transform &  pT)

Normalize data, if needed, to have transform properties.

Parameters
pTthe given Transform
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Pose2D &  pPos 
)

Overloading of operator << for Pose2D.

Parameters
pStreamthe given ostream
pPosthe given Pose2D
Returns
the Pose2D print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Position2D &  pPos 
)

Overloading of operator << for Position2D.

Parameters
pStreamthe given ostream
pPosthe given Position2D
Returns
the Position2D print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Position3D &  pPos 
)

Overloading of operator << for Position3D.

Parameters
pStreamthe given ostream
pPosthe given Position3D
Returns
the Position3D print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Position6D &  pPos 
)

Overloading of operator << for Position6D.

Parameters
pStreamthe given ostream
pPosthe given Position6D
Returns
the Position6D print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const PositionAndVelocity &  pPosVel 
)

Overloading of operator << for PositionAndVelocity.

Parameters
pStreamthe given ostream
pPosVelthe given PositionAndVelocity
Returns
the PositionAndVelocity print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Rotation &  pRot 
)

Overloading of operator << for Rotation.

Parameters
pStreamthe given ostream
pRotthe given Rotation
Returns
the Rotation print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Rotation3D &  pRot 
)

Overloading of operator << for Rotation3D.

Parameters
pStreamthe given ostream
pRotthe given Rotation3D
Returns
the Rotation3D print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Transform &  pT 
)

Overloading of operator << for Transform.

Parameters
pStreamthe given ostream
pTthe given Transform
Returns
the Transform print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const TransformAndVelocity6D &  pTV 
)

Overloading of operator << for TransformAndVelocity6D.

Parameters
pStreamthe given ostream
pTVthe given TransformAndVelocity6D
Returns
the TransformAndVelocity6D print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Velocity3D &  pVel 
)

Overloading of operator << for Velocity3D.

Parameters
pStreamthe given ostream
pVelthe given Velocity3D
Returns
the Velocity3D print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Quaternion &  pQua 
)

Overloading of operator << for Quaternion.

Parameters
pStreamthe given ostream
pQuathe given Quaternion
Returns
the Quaternion print
std::ostream& AL::Math::operator<< ( std::ostream &  pStream,
const Velocity6D &  pVel 
)

Overloading of operator << for Velocity6D.

Parameters
pStreamthe given ostream
pVelthe given Velocity6D
Returns
the Velocity6D print
Pose2D AL::Math::pinv ( const Pose2D &  pPos)

Alternative name for inverse: return the pose2d inverse of the given Pose2D.

Parameters
pPosthe given Pose2D
Transform AL::Math::pinv ( const Transform &  pT)

Alternative name for inverse: return the transform inverse of the given Transform:

$ pT = \left[\begin{array}{cc} R & r \\ 0_{31} & 1 \end{array}\right]$ $ pTOut = \left[\begin{array}{cc} R^t & (-R^t*r) \\ 0_{31} & 1 \end{array}\right]$

Parameters
pTthe given Transform
Pose2D AL::Math::pose2dDiff ( const Pose2D &  pPos1,
const Pose2D &  pPos2 
)

Compute the Pose2D between the actual Pose2D and the one give in argument result:

inverse(pPos1)*pPos2

Parameters
pPos1the first Pose2D
pPos2the second Pose2D
Returns
the Pose2D
Pose2D AL::Math::pose2DInverse ( const Pose2D &  pPos)

Compute the inverse of a Pose2D.

Parameters
pPosthe initial Pose2D
Returns
the inverse Pose2D
void AL::Math::pose2DInverse ( const Pose2D &  pPos,
Pose2D &  pRes 
)

Compute the inverse of a Pose2D.

Parameters
pPosthe initial Pose2D
pResthe inverse Pose2D
void AL::Math::pose2dInvertInPlace ( Pose2D &  pPos)

Inverse the given Pose2D in place:

Parameters
pPosthe given Pose2D
Quaternion AL::Math::quaternionFromAngleAndAxisRotation ( const float  pAngle,
const float  pAxisX,
const float  pAxisY,
const float  pAxisZ 
)

Create a Quaternion initialized with explicit angle and axis rotation.

Parameters
pAnglethe float value for angle in radian around axis rotation
pAxisXthe float value for x value of axis rotation
pAxisYthe float value for y value of axis rotation
pAxisZthe float value for z value of axis rotation
Returns
the Quaternion
void AL::Math::quaternionInverse ( const Quaternion &  pQua,
Quaternion &  pQuaOut 
)

Return the quaternion inverse of the given Quaternion:

Parameters
pQuathe given Quaternion
pQuaOutthe inverse of the given Quaternion
Quaternion AL::Math::quaternionInverse ( const Quaternion &  pQua)

Return the quaternion inverse of the given Quaternion

Parameters
pQuathe given Quaternion
Returns
the Quaternion inverse
Rotation AL::Math::rotationFrom3DRotation ( const float &  pWX,
const float &  pWY,
const float &  pWZ 
)

Create a Rotation initialized with euler angle. Rot = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX)

Parameters
pWXthe float value for euler angle x in radian
pWYthe float value for euler angle y in radian
pWZthe float value for euler angle z in radian
Returns
the Rotation matrix
Rotation AL::Math::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).

Parameters
pAnglethe float value of angle in radian
pXthe X direction of the vector of the rotation
pYthe Y direction of the vector of the rotation
pZthe Z direction of the vector of the rotation
Returns
the Rotation matrix
Rotation AL::Math::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).

Parameters
pACoefficient a of the normalized quaternion
pBCoefficient b of the normalized quaternion
pCCoefficient c of the normalized quaternion
pDCoefficient d of the normalized quaternion
Returns
the Rotation matrix
Rotation AL::Math::rotationFromRotX ( const float  pRotX)

Create a Rotation initialized with explicit rotation around x axis.

$ pRot = \left[\begin{array}{cccc} 1.0 & 0.0 & 0.0 \\ 0.0 & cos(pRotX) & -sin(pRotX) \\ 0.0 & sin(pRotX) & cos(pRotX) \end{array}\right]$

Parameters
pRotXthe float value for angle rotation in radian around x axis
Returns
the Rotation matrix
Rotation AL::Math::rotationFromRotY ( const float  pRotY)

Create a Rotation initialized with explicit rotation around y axis.

$ pT = \left[\begin{array}{cccc} cos(pRotY) & 0.0 & sin(pRotY) \\ 0.0 & 1.0 & 0.0 \\ -sin(pRotY) & 0.0 & cos(pRotY) \end{array}\right]$

Parameters
pRotYthe float value for angle rotation in radian around y axis
Returns
the Rotation matrix
Rotation AL::Math::rotationFromRotZ ( const float  pRotZ)

Create a Rotation initialized with explicit rotation around z axis.

$ pT = \left[\begin{array}{cccc} cos(pRotZ) & -sin(pRotZ) & 0.0 \\ sin(pRotZ) & cos(pRotZ) & 0.0 \\ 0.0 & 0.0 & 1.0 \end{array}\right]$

Parameters
pRotZthe float value for angle rotation in radian around z axis
Returns
the Rotation matrix
std::string AL::Math::toSpaceSeparated ( const Position3D &  pPos)

Create a string of Position3D.

Parameters
pPosthe given Position3D
Returns
the Position3D string
std::string AL::Math::toSpaceSeparated ( const Rotation3D &  pPos)

Create a string of Rotation3D.

Parameters
pPosthe given Rotation3D
Returns
the Rotation3D string
std::string AL::Math::toSpaceSeparated ( const Velocity6D &  pVel)

Create a string of Velocity6D.

Parameters
pVelthe given Velocity6D
Returns
the Velocity6D string
std::string AL::Math::toSpaceSeparated ( const Transform &  pT)

Create a string of Transform.

Parameters
pTthe given Transform
Returns
the Transform string
std::string AL::Math::toSpaceSeparated ( const Position6D &  pPos)

Create a string of Position6D.

Parameters
pPosthe given Position6D
Returns
the Position6D string
std::string AL::Math::toSpaceSeparated ( const Quaternion &  pQuat)

Create a string of Quaternion.

<param name="pQuat> the given Quaternion

Returns
the Quaternion string
Transform AL::Math::transformDiff ( const Transform &  pT1,
const Transform &  pT2 
)

Compute the Transform between the actual Transform and the one give in argument result:

inverse(pT1)*pT2

Parameters
pT1the first transform
pT2the second transform
Returns
the Transform
float AL::Math::transformDistance ( const Transform &  pT1,
const Transform &  pT2 
)

Compute the distance between the actual Transform and the one give in argument:

$\sqrt{(pT1.r_1c_4-pT2.r_1c_4)^2+(pT1.r_2c_4-pT2.r_2c_4)^2+(pT1.r_3c_4-pT2.r_3c_4)^2}$

Parameters
pT1the first Transform
pT2the second Transform
Returns
the float distance between the two Transform
float AL::Math::transformDistanceSquared ( const Transform &  pT1,
const Transform &  pT2 
)

Compute the squared distance between the actual Transform and the one give in argument (translation part):

$(pT1.r_1c_4-pT2.r_1c_4)^2 +(pT1.r_2c_4-pT2.r_2c_4)^2+(pT1.r_3c_4-pT2.r_3c_4)^2$

Parameters
pT1the first Transform
pT2the second Transform
Returns
the float squared distance between the two Transform: translation part
Transform AL::Math::transformFrom3DRotation ( const float &  pWX,
const float &  pWY,
const float &  pWZ 
)

Create a Transform initialize with euler angle: H = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX)

Parameters
pWXthe float value for euler angle x in radian
pWYthe float value for euler angle y in radian
pWZthe float value for euler angle z in radian
Returns
the Transform
Transform AL::Math::transformFromPosition ( const float &  pX,
const float &  pY,
const float &  pZ 
)

Create a Transform initialize with explicit value for translation part:

$ pTOut = \left[\begin{array}{cccc} 1.0 & 0.0 & 0.0 & pX \\ 0.0 & 1.0 & 0.0 & pY \\ 0.0 & 0.0 & 1.0 & pZ \\ 0.0 & 0.0 & 0.0 & 1.0 \end{array}\right]$

Parameters
pXthe float value for translation axis x in meter (r1_c4)
pYthe float value for translation axis y in meter (r2_c4)
pZthe float value for translation axis z in meter (r3_c4)
Returns
the Transform
Transform AL::Math::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:

H = fromRotZ(pWZ)*fromRotY(pWY)*fromRotX(pWX)

H.r1_c4 = pX

H.r2_c4 = pY

H.r3_c4 = pZ

Parameters
pXthe float value for translation axis x in meter (r1_c4)
pYthe float value for translation axis y in meter (r2_c4)
pZthe float value for translation axis z in meter (r3_c4)
pWXthe float value for euler angle x in radian
pWYthe float value for euler angle y in radian
pWZthe float value for euler angle z in radian
Returns
the Transform
Transform AL::Math::transformFromRotX ( const float  pRotX)

Create a Transform initialize with explicit rotation around x axis:

$ pTOut = \left[\begin{array}{cccc} 1.0 & 0.0 & 0.0 & 0.0 \\ 0.0 & cos(pRotX) & -sin(pRotX) & 0.0 \\ 0.0 & sin(pRotX) & cos(pRotX) & 0.0 \\ 0.0 & 0.0 & 0.0 & 1.0 \end{array}\right]$

Parameters
pRotXthe float value for angle rotation in radian around x axis
Returns
the Transform
Transform AL::Math::transformFromRotY ( const float  pRotY)

Create a Transform initialize with explicit rotation around y axis:

$ pTOut = \left[\begin{array}{cccc} cos(pRotY) & 0.0 & sin(pRotY) & 0.0 \\ 0.0 & 1.0 & 0.0 & 0.0 \\ -sin(pRotY) & 0.0 & cos(pRotY) & 0.0 \\ 0.0 & 0.0 & 0.0 & 1.0 \end{array}\right]$

Parameters
pRotYthe float value for angle rotation in radian around y axis
Returns
the Transform
Transform AL::Math::transformFromRotZ ( const float  pRotZ)

Create a Transform initialize with explicit rotation around z axis:

$ pTOut = \left[\begin{array}{cccc} cos(pRotZ) & -sin(pRotZ) & 0.0 & 0.0 \\ sin(pRotZ) & cos(pRotZ) & 0.0 & 0.0 \\ 0.0 & 0.0 & 1.0 & 0.0 \\ 0.0 & 0.0 & 0.0 & 1.0 \end{array}\right]$

Parameters
pRotZthe float value for angle rotation in radian around z axis
Returns
the Transform
void AL::Math::transformInverse ( const Transform &  pT,
Transform &  pTOut 
)

Return the transform inverse of the given Transform:

$ pT = \left[\begin{array}{cc} R & r \\ 0_{31} & 1 \end{array}\right]$ $ pTOut = \left[\begin{array}{cc} R^t & (-R^t*r) \\ 0_{31} & 1 \end{array}\right]$

Parameters
pTthe given Transform
pTOutthe inverse of the given Transform
Transform AL::Math::transformInverse ( const Transform &  pT)

Return the transform inverse of the given Transform:

$ pT = \left[\begin{array}{cc} R & r \\ 0_{31} & 1 \end{array}\right]$ $ pTOut = \left[\begin{array}{cc} R^t & (-R^t*r) \\ 0_{31} & 1 \end{array}\right]$

Parameters
pTthe given Transform
Returns
the Transform inverse
void AL::Math::transformInvertInPlace ( Transform &  pT)

Inverse the given Transform in place:

Parameters
pTthe given Transform
void AL::Math::transformPreMultiply ( const Transform &  pT,
Transform &  pTOut 
)

pTOut = pT*pTOut

Parameters
pTthe first constant Transform
pTOutthe second modified Transform
void AL::Math::transformToFloatVector ( const Transform &  pT,
std::vector< float > &  pTOut 
)

DEPRECATED: Use toVector function. Copy the Transform in a vector of float:

$ \begin{array}{cccc} [r_1c_1, & r_1c_2, & r_1c_3, & r_1c_4, \\ r_2c_1, & r_2c_2, & r_2c_3, & r_2c_4, \\ r_3c_1, & r_3c_2, & r_3c_3, & r_3c_4, \\ 0.0, & 0.0, & 0.0, & 1.0] \end{array}$

Parameters
pTthe given Transform
pTOutthe vector of float update to given transform value
std::vector<float> AL::Math::transformToFloatVector ( const Transform &  pT)

DEPRECATED: Use toVector function. Return the Transform in a vector of float:

$ \begin{array}{cccc} [r_1c_1, & r_1c_2, & r_1c_3, & r_1c_4, \\ r_2c_1, & r_2c_2, & r_2c_3, & r_2c_4, \\ r_3c_1, & r_3c_2, & r_3c_3, & r_3c_4, \\ 0.0, & 0.0, & 0.0, & 1.0] \end{array}$

Parameters
pTthe given Transform
Returns
the vector of float update to given transform value
Rotation AL::Math::transpose ( const Rotation &  pRot)

Compute the transpose rotation of the one given in argument:

Parameters
pRotthe rotation matrix
Returns
the rotation transposed