libalmath  2.5.7.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Functions
Tools

Functions

std::vector< Pose2D > AL::Math::getDubinsSolutions (const Pose2D &pTargetPose, const float pCircleRadius)
 Get the dubins solutions. More...
 
void AL::Math::modulo2PIInPlace (float &pAngle)
 Set an angle between ]-PI, PI]. More...
 
float AL::Math::modulo2PI (float pAngle)
 Return an angle between ]-PI, PI]. More...
 
float AL::Math::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 AL::Math::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 AL::Math::clipData (const float &pMin, const float &pMax, float &pData)
 Clip an input data inside min and max limit. More...
 
void AL::Math::changeReferencePose2D (const float &pTheta, const Pose2D &pPosIn, Pose2D &pPosOut)
 
void AL::Math::changeReferencePose2DInPlace (const float &pTheta, Pose2D &pPosOut)
 Change orientation of a Pose2D in place. More...
 
Position6D AL::Math::position6DFromVelocity6D (const Velocity6D &pVel)
 Create a Position6D from a Velocity6D More...
 
void AL::Math::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 AL::Math::position2DFromPose2D (const Pose2D &pPose2D)
 Create a Position2D from a Pose2D More...
 
Position3D AL::Math::position3DFromPosition6D (const Position6D &pPosition6D)
 Create a Position3D from a Position6D More...
 
Position3D AL::Math::operator* (const Rotation &pRot, const Position3D &pPos)
 Overloading of operator * between Rotation and Position3D: More...
 
Position3D AL::Math::operator* (const Quaternion &pQuat, const Position3D &pPos)
 Overloading of operator * between Quaternion and Position3D More...
 
Velocity6D AL::Math::operator* (const float pVal, const Position6D &pPos)
 Overloading of operator * for float to Position6D, give a Velocity6D: More...
 
Velocity3D AL::Math::operator* (const float pVal, const Position3D &pPos)
 Overloading of operator * for float to Position3D, give a Velocity3D: More...
 
Velocity3D AL::Math::operator* (const Rotation &pRot, const Velocity3D &pVel)
 Overloading of operator * between Rotation and Velocity3D: More...
 
Rotation AL::Math::rotationFromAngleDirection (const float &pTheta, const Position3D &pPos)
 Creates a 3*3 Rotation Matrix from a an angle and a normalized Position3D. More...
 
void AL::Math::position6DFromPose2DInPlace (const Pose2D &pPose2D, Position6D &pPosition6D)
 Compute a Position6D from a Pose2D. More...
 
Position6D AL::Math::position6DFromPose2D (const Pose2D &pPose2D)
 Create a Position6D from a Pose2D. More...
 
void AL::Math::position6DFromPosition3DInPlace (const Position3D &pPosition3D, Position6D &pPosition6D)
 Compute a Position6D from a Position3D. More...
 
Position6D AL::Math::position6DFromPosition3D (const Position3D &pPosition3D)
 Create a Position6D from a Position3D. More...
 
void AL::Math::pose2DFromPosition6DInPlace (const Position6D &pPosition6D, Pose2D &pPose2D)
 Compute a Pose2D from a Position6D. More...
 
Pose2D AL::Math::pose2DFromPosition6D (const Position6D &pPosition6D)
 Create a Pose2D from a Position6D. More...
 
void AL::Math::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 AL::Math::pose2DFromPosition2D (const Position2D &pPosition2D, const float pAngle=0.0f)
 Create a Pose2D from a Position2D. More...
 
Position2D AL::Math::operator* (const Pose2D &pVal, const Position2D &pPos)
 Overloading of operator * for Pose2D to Position2D, give a Position2D: More...
 
void AL::Math::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...
 
void AL::Math::rotationFromQuaternion (const Quaternion &pQua, Rotation &pRot)
 Create a Rotation Matrix from a Quaternion More...
 
void AL::Math::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...
 
void AL::Math::quaternionPosition3DFromPosition6D (const Position6D &pPos6D, Quaternion &pQua, Position3D &pPos3D)
 Convert a Position6D to Quaternion and Position3D More...
 
void AL::Math::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...
 
void AL::Math::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 AL::Math::transformLogarithm (const Transform &pT)
 Compute the logarithme of a transform. Angle must be between $\left[-\pi+0.001, \pi-0.001\right] $ More...
 
Transform AL::Math::velocityExponential (const Velocity6D &pVel)
 
void AL::Math::changeReferenceVelocity6D (const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)
 
void AL::Math::changeReferencePosition6D (const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)
 
void AL::Math::changeReferencePosition3D (const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)
 
void AL::Math::changeReferenceTransposePosition3D (const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)
 
void AL::Math::changeReferenceTransform (const Transform &pT, const Transform &pTIn, Transform &pTOut)
 
void AL::Math::changeReferenceTransposeTransform (const Transform &pT, const Transform &pTIn, Transform &pTOut)
 
void AL::Math::changeReferenceTransposeVelocity6D (const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)
 
void AL::Math::changeReferenceTransposePosition6D (const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)
 
void AL::Math::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 AL::Math::transformMean (const Transform &pTIn1, const Transform &pTIn2, const float &pVal=0.5f)
 Preform a logarithmic mean of pTIn1 and pTIn2. More...
 
Transform AL::Math::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 AL::Math::transformFromRotationPosition3D (const Rotation &pRot, const Position3D &pPos)
 Create a Transform from a Position3D and a Rotation. More...
 
void AL::Math::transformFromPosition3DInPlace (const Position3D &pPosition, Transform &pTransform)
 Modify pTransform to set the translation part to pPosition. More...
 
Transform AL::Math::transformFromPosition3D (const Position3D &pPosition)
 Create a 4*4 transform matrix from cartesian coordinates given in pPosition. More...
 
void AL::Math::transformFromRotationInPlace (const Rotation &pRotation, Transform &pTransform)
 Modify the rotation part of the transform. The translation part of the transform is not modified. More...
 
Transform AL::Math::transformFromRotation (const Rotation &pRotation)
 
void AL::Math::rotationFromTransformInPlace (const Transform &pTransform, Rotation &pRotation)
 Extract the position coordinates from a Transform. More...
 
Rotation AL::Math::rotationFromTransform (const Transform &pTransform)
 
Rotation3D AL::Math::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 AL::Math::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 AL::Math::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 AL::Math::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 AL::Math::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 AL::Math::position6DFromTransformInPlace (const Transform &pT, Position6D &pPos)
 Compute Position6D corresponding to the Transform. More...
 
Position6D AL::Math::position6DFromTransform (const Transform &pT)
 Compute Position6D corresponding to 4*4 Homogenous Transform. More...
 
void AL::Math::transformFromPose2DInPlace (const Pose2D &pPose, Transform &pT)
 Compute a Transform from a Pose2D. More...
 
Transform AL::Math::transformFromPose2D (const Pose2D &pPose)
 Create a Transform from a Pose2D. More...
 
void AL::Math::pose2DFromTransformInPlace (const Transform &pT, Pose2D &pPos)
 Compute a Pose2D from a Transform. More...
 
Pose2D AL::Math::pose2DFromTransform (const Transform &pT)
 Create a Pose2D from a Transform. More...
 
void AL::Math::position2DFromTransformInPlace (const Transform &pT, Position2D &pPos)
 Compute a Position2D from a Transform. More...
 
Position2D AL::Math::position2DFromTransform (const Transform &pT)
 Create a Position2D from a Transform. More...
 
Transform AL::Math::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 AL::Math::transformFromPosition6D (const Position6D &pPosition6D)
 Create a Transform from a Position6D. More...
 
void AL::Math::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 AL::Math::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 AL::Math::position3DFromTransformInPlace (const Transform &pT, Position3D &pPos)
 Compute a Position3D from a Transform. More...
 
Position3D AL::Math::position3DFromTransform (const Transform &pT)
 Create a Position3D from a Transform. More...
 
Rotation3D AL::Math::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 AL::Math::transformFromRotVecInPlace (const int pAxis, const float pTheta, const Position3D &pPos, Transform &pT)
 Compute a Transform from More...
 
Transform AL::Math::transformFromRotVec (const int pAxis, const float pTheta, const Position3D &pPos)
 
void AL::Math::transformFromRotVecInPlace (const Position3D &pPos, Transform &pT)
 
Transform AL::Math::transformFromRotVec (const Position3D &pPos)
 
Transform AL::Math::transformFromRotVec (const int &pAxis, const float &pTheta)
 
const bool AL::Math::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 AL::Math::clipFootWithEllipse (const float &pMaxFootX, const float &pMaxFootY, Pose2D &pMove)
 Clip foot move with ellipsoid function More...
 
static Pose2D AL::Math::Pose2D::fromPolarCoordinates (const float pRadius, const float pAngle)
 Create a Pose2D from polar coordinates. More...
 

Detailed Description

Function Documentation

const bool AL::Math::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.

Parameters
pLFootBoundingBoxvector<Position2D> of the left footBoundingBox.
pRFootBoundingBoxvector<Position2D> of the right footBoundingBox.
pIsLeftSupportBool true if left is the support leg.
pMovethe desired and return Pose2D.
Returns
true if pMove is clamped.
void AL::Math::changeReferencePose2D ( const float &  pTheta,
const Pose2D &  pPosIn,
Pose2D &  pPosOut 
)

$ \left[\begin{array}{c} pPosOut.x \\ pPosOut.y \\ pPosOut.theta \\ \end{array}\right] = \left[\begin{array}{cccccc} cos(pTheta) & -sin(pTheta) & 0 \\ sin(pTheta) & cos(pTheta) & 0 \\ 0 & 0 & 1 \\ \end{array}\right] \left[\begin{array}{c} pPosIn.x \\ pPosIn.y \\ pPosIn.theta \\ \end{array}\right] $

Parameters
pThetathe given angle in radian
pPosIna input Pose2D
pPosOutthe output Pose2D
void AL::Math::changeReferencePose2DInPlace ( const float &  pTheta,
Pose2D &  pPosOut 
)

Change orientation of a Pose2D in place.

Parameters
pThetathe given angle in radian
pPosOutthe input output Pose2D
void AL::Math::changeReferencePosition3D ( const Transform &  pT,
const Position3D &  pPosIn,
Position3D &  pPosOut 
)

$ \left[\begin{array}{c} pPosOut.x \\ pPosOut.y \\ pPosOut.z \\ \end{array}\right] = \left[\begin{array}{ccc} r_1c_1 & r_1c_2 & r_1c_3\\ r_2c_1 & r_2c_2 & r_2c_3\\ r_3c_1 & r_3c_2 & r_3c_3\\ \end{array}\right] \left[\begin{array}{c} pPosIn.x \\ pPosIn.y \\ pPosIn.z \\ \end{array}\right] $

Parameters
pTthe given Transform
pPosIna Position3D containing the position to change
pPosOuta Position3D containing the changed position
void AL::Math::changeReferencePosition6D ( const Transform &  pT,
const Position6D &  pPosIn,
Position6D &  pPosOut 
)

$ \left[\begin{array}{c} pPOut.x \\ pPOut.y \\ pPOut.z \\ pPOut.wx \\ pPOut.wy \\ pPOut.wz \\ \end{array}\right] = \left[\begin{array}{cccccc} r_1c_1 & r_1c_2 & r_1c_3 & 0 & 0 & 0 \\ r_2c_1 & r_2c_2 & r_2c_3 & 0 & 0 & 0 \\ r_3c_1 & r_3c_2 & r_3c_3 & 0 & 0 & 0 \\ 0 & 0 & 0 & r_1c_1 & r_1c_2 & r_1c_3 \\ 0 & 0 & 0 & r_2c_1 & r_2c_2 & r_2c_3 \\ 0 & 0 & 0 & r_3c_1 & r_3c_2 & r_3c_3 \\ \end{array}\right] \left[\begin{array}{c} pPIn.x \\ pPIn.y \\ pPIn.z \\ pPIn.wx \\ pPIn.wy \\ pPIn.wz \\ \end{array}\right] $

Parameters
pTthe given Transform
pPosIna Position6D containing the position to change
pPosOuta Position6D containing the changed position
void AL::Math::changeReferenceTransform ( const Transform &  pT,
const Transform &  pTIn,
Transform &  pTOut 
)

$ \left[\begin{array}{c} pTOut \\ \end{array}\right] = \left[\begin{array}{c} pT \end{array}\right] \left[\begin{array}{c} pTIn \\ \end{array}\right] $

Parameters
pTthe given Transform
pTInthe Transform to change
pTOutthe changed Transform
void AL::Math::changeReferenceTransposePosition3D ( const Transform &  pT,
const Position3D &  pPosIn,
Position3D &  pPosOut 
)

$ \left[\begin{array}{c} pTOut \\ \end{array}\right] = \left[\begin{array}{c} pT \end{array}\right] \left[\begin{array}{c} pTIn \\ \end{array}\right]^T $

Parameters
pTthe given Transform
pPosIna Position3D containing the position to change
pPosOuta Position3D containing the changed position
void AL::Math::changeReferenceTransposePosition6D ( const Transform &  pT,
const Position6D &  pPosIn,
Position6D &  pPosOut 
)

$ \left[\begin{array}{c} pPOut.x \\ pPOut.y \\ pPOut.z \\ pPOut.wx \\ pPOut.wy \\ pPOut.wz \\ \end{array}\right] = \left[\begin{array}{cccccc} r_1c_1 & r_2c_1 & r_3c_1 & 0 & 0 & 0 \\ r_1c_2 & r_2c_2 & r_3c_2 & 0 & 0 & 0 \\ r_1c_3 & r_2c_3 & r_3c_3 & 0 & 0 & 0 \\ 0 & 0 & 0 & r_1c_1 & r_2c_1 & r_3c_1 \\ 0 & 0 & 0 & r_1c_2 & r_2c_2 & r_3c_2 \\ 0 & 0 & 0 & r_1c_3 & r_2c_3 & r_3c_3 \\ \end{array}\right] \left[\begin{array}{c} pPIn.x \\ pPIn.y \\ pPIn.z \\ pPIn.wx \\ pPIn.wy \\ pPIn.wz \\ \end{array}\right] $

Parameters
pTthe given Transform
pPosIna Position6D containing the position to change
pPosOuta Position6D containing the changed position
void AL::Math::changeReferenceTransposeTransform ( const Transform &  pT,
const Transform &  pTIn,
Transform &  pTOut 
)

$ \left[\begin{array}{c} pTOut \\ \end{array}\right] = \left[\begin{array}{c} pT \end{array}\right] \left[\begin{array}{c} pTIn \\ \end{array}\right]^T $

Parameters
pTthe given Transform
pTInthe Transform to change
pTOutthe changed Transform
void AL::Math::changeReferenceTransposeVelocity6D ( const Transform &  pT,
const Velocity6D &  pVelIn,
Velocity6D &  pVelOut 
)

$ \left[\begin{array}{c} pVOut.xd \\ pVOut.yd \\ pVOut.zd \\ pVOut.wxd \\ pVOut.wyd \\ pVOut.wzd \\ \end{array}\right] = \left[\begin{array}{cccccc} r_1c_1 & r_2c_1 & r_3c_1 & 0 & 0 & 0 \\ r_1c_2 & r_2c_2 & r_3c_2 & 0 & 0 & 0 \\ r_1c_3 & r_2c_3 & r_3c_3 & 0 & 0 & 0 \\ 0 & 0 & 0 & r_1c_1 & r_2c_1 & r_3c_1 \\ 0 & 0 & 0 & r_1c_2 & r_2c_2 & r_3c_2 \\ 0 & 0 & 0 & r_1c_3 & r_2c_3 & r_3c_3 \\ \end{array}\right] \left[\begin{array}{c} pVIn.xd \\ pVIn.yd \\ pVIn.zd \\ pVIn.wxd \\ pVIn.wyd \\ pVIn.wzd \\ \end{array}\right] $

Parameters
pTthe given Transform
pVelIna Velocity6D containing the velocity to change
pVelOuta Velocity6D containing the changed velocity
void AL::Math::changeReferenceVelocity6D ( const Transform &  pT,
const Velocity6D &  pVelIn,
Velocity6D &  pVelOut 
)

$ \left[\begin{array}{c} pVOut.xd \\ pVOut.yd \\ pVOut.zd \\ pVOut.wxd \\ pVOut.wyd \\ pVOut.wzd \\ \end{array}\right] = \left[\begin{array}{cccccc} r_1c_1 & r_1c_2 & r_1c_3 & 0 & 0 & 0 \\ r_2c_1 & r_2c_2 & r_2c_3 & 0 & 0 & 0 \\ r_3c_1 & r_3c_2 & r_3c_3 & 0 & 0 & 0 \\ 0 & 0 & 0 & r_1c_1 & r_1c_2 & r_1c_3 \\ 0 & 0 & 0 & r_2c_1 & r_2c_2 & r_2c_3 \\ 0 & 0 & 0 & r_3c_1 & r_3c_2 & r_3c_3 \\ \end{array}\right] \left[\begin{array}{c} pVIn.xd \\ pVIn.yd \\ pVIn.zd \\ pVIn.wxd \\ pVIn.wyd \\ pVIn.wzd \\ \end{array}\right] $

Parameters
pTthe given Transform
pVelIna Velocity6D containing the velocity to change
pVelOuta Velocity6D containing the changed velocity
bool AL::Math::clipData ( const float &  pMin,
const float &  pMax,
float &  pData 
)

Clip an input data inside min and max limit.

$ pMin \leq pData \leq pMax $

Parameters
pMinthe min limit
pMaxthe max limit
pDatathe clipped data
Returns
Return true if the input pData is clipped.
const bool AL::Math::clipFootWithEllipse ( const float &  pMaxFootX,
const float &  pMaxFootY,
Pose2D &  pMove 
)

Clip foot move with ellipsoid function

Parameters
pMaxFootXfloat of the max step along x axis.
pMaxFootYfloat of the max step along y axis.
pMovethe desired and return Pose2D.
Returns
true if pMove is clamped.
static Pose2D AL::Math::Pose2D::fromPolarCoordinates ( const float  pRadius,
const float  pAngle 
)
static

Create a Pose2D from polar coordinates.

Parameters
pRadiusthe polar radius
pAnglethe polar angle in radians
Returns
the Pose2D extracted from the polar coordinates
std::vector<Pose2D> AL::Math::getDubinsSolutions ( const Pose2D &  pTargetPose,
const float  pCircleRadius 
)

Get the dubins solutions.

Parameters
pTargetPosethe target pose
pCircleRadiusthe circle radius
Returns
The dubins solution.
float AL::Math::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.

Parameters
pAnglesthe input/output angles
Returns
The computed mean (in ]-PI, PI]).
float AL::Math::modulo2PI ( float  pAngle)

Return an angle between ]-PI, PI].

Parameters
pAnglethe input angle
Returns
Return an angle between ]-PI, PI].
void AL::Math::modulo2PIInPlace ( float &  pAngle)

Set an angle between ]-PI, PI].

Parameters
pAnglethe input/output angle
Position3D AL::Math::operator* ( const Rotation &  pRot,
const Position3D &  pPos 
)

Overloading of operator * between Rotation and Position3D:

$\left[\begin{array}{c} result.x \\ result.y \\ result.z \end{array}\right] = \left[\begin{array}{ccc} pRot.r_1c_1 & pRot.r_1c_2 & pRot.r_1c_3 \\ pRot.r_2c_1 & pRot.r_2c_2 & pRot.r_2c_3 \\ pRot.r_3c_1 & pRot.r_3c_2 & pRot.r_3c_3 \end{array}\right] * \left[\begin{array}{c} pPos.x \\ pPos.y \\ pPos.z \end{array}\right] $

Parameters
pRotthe given Rotation
pPosthe given Position3D
Returns
the Position3D result.
Position3D AL::Math::operator* ( const Quaternion &  pQuat,
const Position3D &  pPos 
)

Overloading of operator * between Quaternion and Position3D

Parameters
pQuatthe given Quaternion
pPosthe given Position3D
Returns
the Position3D result.
Velocity6D AL::Math::operator* ( const float  pVal,
const Position6D &  pPos 
)

Overloading of operator * for float to Position6D, give a Velocity6D:

$\begin{array}{ccc} pVel.xd & = & pVal*pPos.x \\ pVel.yd & = & pVal*pPos.y \\ pVel.zd & = & pVal*pPos.z \\ pVel.wxd & = & pVal*pPos.wx \\ pVel.wyd & = & pVal*pPos.wy \\ pVel.wzd & = & pVal*pPos.wz \end{array} $

Parameters
pValthe given float
pPosthe given Position6D
Returns
the Velocity6D
Velocity3D AL::Math::operator* ( const float  pVal,
const Position3D &  pPos 
)

Overloading of operator * for float to Position3D, give a Velocity3D:

$\begin{array}{ccc} pVel.xd & = & pVal*pPos.x \\ pVel.yd & = & pVal*pPos.y \\ pVel.zd & = & pVal*pPos.z \\ \end{array} $

Parameters
pValthe given float
pPosthe given Position3D
Returns
the Velocity3D
Velocity3D AL::Math::operator* ( const Rotation &  pRot,
const Velocity3D &  pVel 
)

Overloading of operator * between Rotation and Velocity3D:

$\left[\begin{array}{c} result.x \\ result.y \\ result.z \end{array}\right] = \left[\begin{array}{ccc} pRot.r_1c_1 & pRot.r_1c_2 & pRot.r_1c_3 \\ pRot.r_2c_1 & pRot.r_2c_2 & pRot.r_2c_3 \\ pRot.r_3c_1 & pRot.r_3c_2 & pRot.r_3c_3 \end{array}\right] * \left[\begin{array}{c} pVel.x \\ pVel.y \\ pVel.z \end{array}\right] $

Parameters
pRotthe given Rotation
pVelthe given Velocity3D
Returns
the Velocity3D result.
Position2D AL::Math::operator* ( const Pose2D &  pVal,
const Position2D &  pPos 
)

Overloading of operator * for Pose2D to Position2D, give a Position2D:

$\begin{array}{ccc} pRes.x & = & pVal.x + cos(pVal.theta)*pPos.x - sin(pVal.theta)*pPos.y \\ pRes.y & = & pVal.y + sin(pVal.theta)*pPos.x + cos(pVal.theta)*pPos.y \\ \end{array} $

Parameters
pValthe given Pose2D
pPosthe given Position2D
Returns
the Position2D
void AL::Math::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

$ \left[\begin{array}{ccc} m (y^2 + z^2) & -m x y & -m x z \\ -m x y & m (x^2 + z^2) & -m y z \\ -m x z & -m y z & m (x^2 + y^2) \\ \end{array}\right] $

Parameters
pMassmass of the point
pPosposition of the point
pInertiaa vector of size 9, rotational inertia matrix of the point mass, expressed at the origin

Thanks to the Huygens–Steiner theorem, this function can be used to change the reference of a rigid-body (non-necessarily punctual) rotational inertia matrix:

typedef std::vector<float> Inertia; float mass = ...; Inertia inertiaAtCom = ...; AL::Math::Position3d comPosition = ...; Inertia inertiaAtOrigin; // first compute the contribution of the translation pointMassRotationalInertia(mass, comPosition, inertiaAtOrigin); // then add the "proper" body inertia std::transform(inertiaAtCom.begin(), inertiaAtCom.end(), inertiaAtOrigin.begin(), inertiaAtOrigin.begin(), std::plus<float>());

Pose2D AL::Math::pose2DFromPosition2D ( const Position2D &  pPosition2D,
const float  pAngle = 0.0f 
)

Create a Pose2D from a Position2D.

Parameters
pPosition2Dthe Position2D you want to extract
pAnglethe angle in radians to set the new Pose2D to
Returns
the Pose2D extracted from the Position2D
void AL::Math::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

Parameters
pPosition2Dthe Position2D you want to extract
pAnglethe angle in radians to set pPose2d to
pPose2Dthe result Pose2D
Pose2D AL::Math::pose2DFromPosition6D ( const Position6D &  pPosition6D)

Create a Pose2D from a Position6D.

Parameters
pPosition6Dthe position6d you want to extract
Returns
the Pose2D extracted from the Position6D
void AL::Math::pose2DFromPosition6DInPlace ( const Position6D &  pPosition6D,
Pose2D &  pPose2D 
)

Compute a Pose2D from a Position6D.

Parameters
pPosition6Dthe Position6D you want to extract
pPose2Dthe result Pose2D
Pose2D AL::Math::pose2DFromTransform ( const Transform &  pT)

Create a Pose2D from a Transform.

Parameters
pTthe transform you want to extract
Returns
the Pose2D extracted from the Transform
void AL::Math::pose2DFromTransformInPlace ( const Transform &  pT,
Pose2D &  pPos 
)

Compute a Pose2D from a Transform.

Parameters
pTthe Transform you want to extract
pPosthe result Pose2D
Position2D AL::Math::position2DFromPose2D ( const Pose2D &  pPose2D)

Create a Position2D from a Pose2D

$\begin{array}{ccc} result.x & = & pPose2d.x \\ result.y & = & pPose2d.y \\ \end{array} $

Parameters
pPose2Dthe given Pose2D
Returns
the Position2D result.
void AL::Math::position2DFromPose2DInPlace ( const Pose2D &  pPose2D,
Position2D &  pPosition2D 
)

Compute a Position2D from a Pose2D. The theta member of the Pose2D is not taken into account.

Parameters
pPose2Dthe Pose2D to extract
pPosition2Dthe result Position2D
Position2D AL::Math::position2DFromTransform ( const Transform &  pT)

Create a Position2D from a Transform.

Parameters
pTthe transform you want to extract
Returns
the Position2D extracted from the Transform
void AL::Math::position2DFromTransformInPlace ( const Transform &  pT,
Position2D &  pPos 
)

Compute a Position2D from a Transform.

Parameters
pTthe Transform you want to extract
pPosthe result Position2D
Position3D AL::Math::position3DFromPosition6D ( const Position6D &  pPosition6D)

Create a Position3D from a Position6D

$\begin{array}{ccc} result.x & = & pPose6d.x \\ result.y & = & pPose6d.y \\ result.z & = & pPose6d.z \\ \end{array} $

Parameters
pPosition6Dthe given Position6D
Returns
the Position3D result.
Position3D AL::Math::position3DFromTransform ( const Transform &  pT)

Create a Position3D from a Transform.

$ \left[\begin{array}{c} return.x \\ return.y \\ return.z \\ \end{array}\right] = \left[\begin{array}{c} pT.r_1c_4 \\ pT.r_2c_4 \\ pT.r_3c_4 \\ \end{array}\right] $

Parameters
pTthe Transform you want to extract
Returns
the result Position6D
void AL::Math::position3DFromTransformInPlace ( const Transform &  pT,
Position3D &  pPos 
)

Compute a Position3D from a Transform.

$ \left[\begin{array}{c} pPos.x \\ pPos.y \\ pPos.z \\ \end{array}\right] = \left[\begin{array}{c} pT.r_1c_4 \\ pT.r_2c_4 \\ pT.r_3c_4 \\ \end{array}\right] $

Parameters
pTthe Transform you want to extract
pPosthe result Position3D
Position6D AL::Math::position6DFromPose2D ( const Pose2D &  pPose2D)

Create a Position6D from a Pose2D.

Parameters
pPose2Dthe pose2D you want to extract
Returns
the result Position6D
void AL::Math::position6DFromPose2DInPlace ( const Pose2D &  pPose2D,
Position6D &  pPosition6D 
)

Compute a Position6D from a Pose2D.

Parameters
pPose2Dthe Pose2D to extract
pPosition6Dthe result Position6D
Position6D AL::Math::position6DFromPosition3D ( const Position3D &  pPosition3D)

Create a Position6D from a Position3D.

Parameters
pPosition3Dthe position3D you want to extract
Returns
the result Position6D
void AL::Math::position6DFromPosition3DInPlace ( const Position3D &  pPosition3D,
Position6D &  pPosition6D 
)

Compute a Position6D from a Position3D.

Parameters
pPosition3Dthe Position3D to extract
pPosition6Dthe result Position6D
Position6D AL::Math::position6DFromTransform ( const Transform &  pT)

Compute Position6D corresponding to 4*4 Homogenous Transform.

Parameters
pTthe transform you want to extract
Returns
the extracted Position6D
Position6D AL::Math::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.

Parameters
pCurrentthe Position6D you want to extract
pTargetthe Position6D you want to extract
Returns
the result Position6D
void AL::Math::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.

For instance, one would do

Position6D P;
position6DFromTransformDiffInPlace(H_ab, H_ac, P)

Now P contains (an approximation of) the dispacement from the frame b to the frame c, expressed at the origin of frame b, and in the basis of frame a

Parameters
pCurrentthe Position6D you want to extract
pTargetthe Position6D you want to extract
resultthe result Position6D
void AL::Math::position6DFromTransformInPlace ( const Transform &  pT,
Position6D &  pPos 
)

Compute Position6D corresponding to the Transform.

Parameters
pTthe transform you want to extract
pPosthe transform you want to extract
Position6D AL::Math::position6DFromVelocity6D ( const Velocity6D &  pVel)

Create a Position6D from a Velocity6D

$\begin{array}{ccc} result.x & = & pVel.xd \\ result.y & = & pVel.yd \\ result.z & = & pVel.zd \\ result.wx & = & pVel.wxd \\ result.wy & = & pVel.wyd \\ result.wz & = & pVel.wzd \end{array} $

Parameters
pVelthe given Velocity6D
Returns
the Position6D result.
void AL::Math::quaternionFromRotation3D ( const Rotation3D &  pRot3D,
Quaternion &  pQuaternion 
)

Create a Quaternion from a Rotation3D when composed in the following order: Rz(wz) * Ry(wy) * Rx(wx)

Parameters
pRot3Dthe rotation3d you want to extract
Returns
the Quaternion extracted from the Rotation3D
void AL::Math::quaternionPosition3DFromPosition6D ( const Position6D &  pPos6D,
Quaternion &  pQua,
Position3D &  pPos3D 
)

Convert a Position6D to Quaternion and Position3D

Parameters
pPos6Dthe input Position6D you want to extract
pQuathe Quaternion extracted from Position6D
pPos3Dthe Position3D extracted from Position6D
void AL::Math::rotation3DFromQuaternion ( const Quaternion &  pQuaterion,
Rotation3D &  pRot3D 
)

Create a Rotation3D from a Quaternion when composed in the following order: Rz(wz) * Ry(wy) * Rx(wx)

Parameters
pQuaternionthe quaternion you want to extract
Returns
the Rotation3D extracted from the Quaternion
Rotation3D AL::Math::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

Parameters
pRotationthe Rotation to extract
Returns
the Rotation3D extracted from pRotation
Rotation3D AL::Math::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)

Parameters
pTthe Transform you want to extract
Returns
the result Rotation3D
Rotation AL::Math::rotationFromAngleDirection ( const float &  pTheta,
const Position3D &  pPos 
)

Creates a 3*3 Rotation Matrix from a an angle and a normalized Position3D.

Parameters
pThetathe float value of angle in radian
pPosthe Position3D direction of the vector of the rotation, normalized
Returns
the Rotation matrix
Rotation AL::Math::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.

Parameters
pXdirection vector for X axis.
pYdirection vector for Y axis.
Returns
the Rotation matrix
Rotation AL::Math::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.

Parameters
pXdirection vector for X axis.
pYdirection vector for Y axis.
pZdirection vector for Z axis.
Returns
the Rotation matrix
Rotation AL::Math::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.

Parameters
pXdirection vector for X axis.
pZdirection vector for Z axis.
Returns
the Rotation matrix
Rotation AL::Math::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.

Parameters
pYdirection vector for Y axis.
pZdirection vector for Z axis.
Returns
the Rotation matrix
void AL::Math::rotationFromQuaternion ( const Quaternion &  pQua,
Rotation &  pRot 
)

Create a Rotation Matrix from a Quaternion

Parameters
pQuathe quaternion you want to extract
Returns
the Rotation matrix extracted from the Quaternion
Rotation AL::Math::rotationFromTransform ( const Transform &  pTransform)

Parameters
pTransformposition in cartesian coordinates
Returns
the Rotation extracted from the Transform
void AL::Math::rotationFromTransformInPlace ( const Transform &  pTransform,
Rotation &  pRotation 
)

Extract the position coordinates from a Transform.

Parameters
pTransformthe given transform
pRotationa Rotation to be set with the reslut
Transform AL::Math::transformFromPose2D ( const Pose2D &  pPose)

Create a Transform from a Pose2D.

Parameters
pPosethe pose2D you want to extract
Returns
the result Transform
void AL::Math::transformFromPose2DInPlace ( const Pose2D &  pPose,
Transform &  pT 
)

Compute a Transform from a Pose2D.

Parameters
pPosethe Pose2D to extract
pTthe result Transform
Transform AL::Math::transformFromPosition3D ( const Position3D &  pPosition)

Create a 4*4 transform matrix from cartesian coordinates given in pPosition.

$ \left[\begin{array}{c} Transform \\ \end{array}\right] = \left[\begin{array}{cc} pRotation& *_{3,1}\\ 0_{1,3} & 1\\ \end{array}\right] $

Parameters
pPositionposition in cartesian coordinates
Returns
a Transform with the translation part initialized to pPosition, rotation part is set to identity
void AL::Math::transformFromPosition3DInPlace ( const Position3D &  pPosition,
Transform &  pTransform 
)

Modify pTransform to set the translation part to pPosition.

$ \left[\begin{array}{c} Transform \\ \end{array}\right] = \left[\begin{array}{cc} pRotation& *_{3,1}\\ 0_{1,3} & 1\\ \end{array}\right] $

Parameters
pPositiona Position3D cartesian coordinates
pTransformthe given Transform
Transform AL::Math::transformFromPosition6D ( const Position6D &  pPosition6D)

Create a Transform from a Position6D.

Parameters
pPosition6Dthe Position6D you want to extract
Returns
the result Transform
Transform AL::Math::transformFromRotation ( const Rotation &  pRotation)

$ \left[\begin{array}{c} Transform \\ \end{array}\right] = \left[\begin{array}{cc} pRotation& 0_{3,1}\\ 0_{1,3} & 1\\ \end{array}\right] $

Parameters
pRotationthe given Rotation
Returns
a Transform with the rotation part initialized to pRotation
Transform AL::Math::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

Parameters
pRotationthe Rotation you want to extract
Returns
the result Transform
void AL::Math::transformFromRotationInPlace ( const Rotation &  pRotation,
Transform &  pTransform 
)

Modify the rotation part of the transform. The translation part of the transform is not modified.

$ \left[\begin{array}{c} Transform \\ \end{array}\right] = \left[\begin{array}{cc} pRotation& *_{3,1}\\ 0_{1,3} & 1\\ \end{array}\right] $

Parameters
pRotationthe given Rotation
pTransformthe Transform to modify
Transform AL::Math::transformFromRotationPosition3D ( const Rotation &  pRot,
const float &  pX,
const float &  pY,
const float &  pZ 
)

Create a Transform from 3D cartesian coordiantes and a Rotation.

$ T = \left[\begin{array}{cccc} pRot.r_1c_1 & pRot.r_1c_2 & pRot.r_1c_3 & pX \\ pRot.r_2c_1 & pRot.r_2c_2 & pRot.r_2c_3 & pY \\ pRot.r_3c_1 & pRot.r_3c_2 & pRot.r_3c_3 & pZ \\ 0.0 & 0.0 & 0.0 & 1.0 \end{array}\right] $

Parameters
pRotthe given Rotation
pXthe translation along x axis
pYthe translation along y axis
pZthe translation along z axis
Returns
the Transform result.
Transform AL::Math::transformFromRotationPosition3D ( const Rotation &  pRot,
const Position3D &  pPos 
)

Create a Transform from a Position3D and a Rotation.

$ T = \left[\begin{array}{cccc} pRot.r_1c_1 & pRot.r_1c_2 & pRot.r_1c_3 & pPos.x \\ pRot.r_2c_1 & pRot.r_2c_2 & pRot.r_2c_3 & pPos.y \\ pRot.r_3c_1 & pRot.r_3c_2 & pRot.r_3c_3 & pPos.z \\ 0.0 & 0.0 & 0.0 & 1.0 \end{array}\right] $

Parameters
pPosthe given Position3D
pRotthe given Rotation
Returns
the Transform result.
Transform AL::Math::transformFromRotVec ( const int  pAxis,
const float  pTheta,
const Position3D &  pPos 
)

Parameters
pAxisthe Rotation you want to extract
pThetathe Rotation you want to extract
pPosthe Rotation you want to extract
Returns
the result Transform
Transform AL::Math::transformFromRotVec ( const Position3D &  pPos)

Parameters
pPosthe Rotation you want to extract
Returns
the result Transform
Transform AL::Math::transformFromRotVec ( const int &  pAxis,
const float &  pTheta 
)

Parameters
pAxisthe Rotation you want to extract
pThetathe Rotation you want to extract
Returns
the result Transform
void AL::Math::transformFromRotVecInPlace ( const int  pAxis,
const float  pTheta,
const Position3D &  pPos,
Transform &  pT 
)

Compute a Transform from

Parameters
pAxisthe Rotation you want to extract
pThetathe rotation you want to extract
pPosthe Position3D you want to extract
pTthe Rotation you want to extract
void AL::Math::transformFromRotVecInPlace ( const Position3D &  pPos,
Transform &  pT 
)

Parameters
pPosthe Rotation you want to extract
pTthe Rotation you want to extract
Velocity6D AL::Math::transformLogarithm ( const Transform &  pT)

Compute the logarithme of a transform. Angle must be between $\left[-\pi+0.001, \pi-0.001\right] $

Cette fonction calcule le logarithme associe a une matrice de type Deplacement - matrice homogene 4x4 (SE3) La sortie est un torseur cinematique de se3. Le resultat n'est garanti que pour des angles dans [-pi+0.001,pi-0.001]. cette fonction calcule la differentielle du logarithme associe a une matrice de type Deplacement - matrice homogene 4x4 (SE3).

Parameters
pTthe given Transform
Returns
the Velocity6D logarithme: kinematic screw in se3
void AL::Math::transformLogarithmInPlace ( const Transform &  pT,
Velocity6D &  pVel 
)

Compute the logarithme of a transform. Angle must be between $\left[-\pi+0.001, \pi-0.001\right]$

Cette fonction calcule le logarithme associe a une matrice de type Deplacement - matrice homogene 4x4 (SE3) La sortie est un torseur cinematique de se3. Le resultat n'est garanti que pour des angles dans [-pi+0.001,pi-0.001]. cette fonction calcule la differentielle du logarithme associe a une matrice de type Deplacement - matrice homogene 4x4 (SE3).

Parameters
pTthe given Transform
pVelthe given Transform
Returns
the Velocity6D logarithme: kinematic screw in se3
Transform AL::Math::transformMean ( const Transform &  pTIn1,
const Transform &  pTIn2,
const float &  pVal = 0.5f 
)

Preform a logarithmic mean of pTIn1 and pTIn2.

Parameters
pTIn1cartesian coordinates
pTIn2the given Transform
pValthe value between 0 and 1 used in the logarithmic mean, 0.5 by default
Returns
a Transform with the mean of pTIn1 and pTIn2
void AL::Math::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.

Parameters
pTIn1the first given Transform
pTIn2the second given Transform
pValthe value between 0 and 1 used in the logarithmic mean
pTOutthe output Transform.
Transform AL::Math::velocityExponential ( const Velocity6D &  pVel)

Function Velocity Exponential: compute homogenous matrix displacement from a dt * 6D velocity vector.

Parameters
pVelthe given Velocity6D
Returns
the Velocity6D logarithme: kinematic screw in se3
float AL::Math::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.

All weights must be strictly positive, else the method throws.

Parameters
pAnglesthe input/output angles
pWeightsthe input/output weights
Returns
The computed mean (in ]-PI, PI]).