libalmath  2.1.4.13
 All Classes Namespaces Functions Variables Typedefs 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...
 
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...
 
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

Definition at line 34 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 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 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 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 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 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 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
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 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 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 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
Position3D AL::Math::normalize ( const Position3D &  pPos)

Normalize a Position3D:

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

Parameters
pPosthe given Position3D
Returns
the given Position3D 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
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