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)
	Compute the logarithme of a transform. Angle must be between $\left[-\pi+0.001, \pi-0.001\right]$ More...

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 44 Homogenous transform matrix Current to a 44 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 44 Homogenous transform matrix Current to a 44 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

pLFootBoundingBox	vector<Position2D> of the left footBoundingBox.
pRFootBoundingBox	vector<Position2D> of the right footBoundingBox.
pIsLeftSupport	Bool true if left is the support leg.
pMove	the 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

pTheta	the given angle in radian
pPosIn	a input Pose2D
pPosOut	the output Pose2D

void AL::Math::changeReferencePose2DInPlace	(	const float &	pTheta,
		Pose2D &	pPosOut
	)

Change orientation of a Pose2D in place.

Parameters

pTheta	the given angle in radian
pPosOut	the 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

pT	the given Transform
pPosIn	a Position3D containing the position to change
pPosOut	a 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

pT	the given Transform
pPosIn	a Position6D containing the position to change
pPosOut	a 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

pT	the given Transform
pTIn	the Transform to change
pTOut	the 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

pT	the given Transform
pPosIn	a Position3D containing the position to change
pPosOut	a 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

pT	the given Transform
pPosIn	a Position6D containing the position to change
pPosOut	a 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

pT	the given Transform
pTIn	the Transform to change
pTOut	the 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

pT	the given Transform
pVelIn	a Velocity6D containing the velocity to change
pVelOut	a 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

pT	the given Transform
pVelIn	a Velocity6D containing the velocity to change
pVelOut	a 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

pMin	the min limit
pMax	the max limit
pData	the 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

pMaxFootX	float of the max step along x axis.
pMaxFootY	float of the max step along y axis.
pMove	the 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

pRadius	the polar radius
pAngle	the 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

pTargetPose	the target pose
pCircleRadius	the 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

pAngles the input/output angles

Returns: The computed mean (in ]-PI, PI]).

float AL::Math::modulo2PI ( float pAngle )

Return an angle between ]-PI, PI].

Parameters

pAngle the input angle

Returns: Return an angle between ]-PI, PI].

void AL::Math::modulo2PIInPlace ( float & pAngle )

Set an angle between ]-PI, PI].

Parameters

pAngle the 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

pRot	the given Rotation
pPos	the given Position3D

Returns: the Position3D result.

Position3D AL::Math::operator*	(	const Quaternion &	pQuat,
		const Position3D &	pPos
	)

Overloading of operator * between Quaternion and Position3D

Parameters

pQuat	the given Quaternion
pPos	the 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

pVal	the given float
pPos	the 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

pVal	the given float
pPos	the 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

pRot	the given Rotation
pVel	the 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

pVal	the given Pose2D
pPos	the 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

pMass	mass of the point
pPos	position of the point
pInertia	a 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

pPosition2D	the Position2D you want to extract
pAngle	the 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

pPosition2D	the Position2D you want to extract
pAngle	the angle in radians to set pPose2d to
pPose2D	the result Pose2D

Pose2D AL::Math::pose2DFromPosition6D ( const Position6D & pPosition6D )

Create a Pose2D from a Position6D.

Parameters

pPosition6D the 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

pPosition6D	the Position6D you want to extract
pPose2D	the result Pose2D

Pose2D AL::Math::pose2DFromTransform ( const Transform & pT )

Create a Pose2D from a Transform.

Parameters

pT	the 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

pT	the Transform you want to extract
pPos	the 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

pPose2D the 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

pPose2D	the Pose2D to extract
pPosition2D	the result Position2D

Position2D AL::Math::position2DFromTransform ( const Transform & pT )

Create a Position2D from a Transform.

Parameters

pT	the 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

pT	the Transform you want to extract
pPos	the 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

pPosition6D the 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

pT	the 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

pT	the Transform you want to extract
pPos	the result Position3D

Position6D AL::Math::position6DFromPose2D ( const Pose2D & pPose2D )

Create a Position6D from a Pose2D.

Parameters

pPose2D the 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

pPose2D	the Pose2D to extract
pPosition6D	the result Position6D

Position6D AL::Math::position6DFromPosition3D ( const Position3D & pPosition3D )

Create a Position6D from a Position3D.

Parameters

pPosition3D the 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

pPosition3D	the Position3D to extract
pPosition6D	the result Position6D

Position6D AL::Math::position6DFromTransform ( const Transform & pT )

Compute Position6D corresponding to 4*4 Homogenous Transform.

Parameters

pT	the 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

pCurrent	the Position6D you want to extract
pTarget	the 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

pCurrent	the Position6D you want to extract
pTarget	the Position6D you want to extract
result	the result Position6D

void AL::Math::position6DFromTransformInPlace	(	const Transform &	pT,
		Position6D &	pPos
	)

Compute Position6D corresponding to the Transform.

Parameters

pT	the transform you want to extract
pPos	the 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

pVel	the 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

pRot3D the 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

pPos6D	the input Position6D you want to extract
pQua	the Quaternion extracted from Position6D
pPos3D	the 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

pQuaternion the 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

pRotation the 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

pT	the 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

pTheta	the float value of angle in radian
pPos	the 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

pX	direction vector for X axis.
pY	direction 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

pX	direction vector for X axis.
pY	direction vector for Y axis.
pZ	direction 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

pX	direction vector for X axis.
pZ	direction 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

pY	direction vector for Y axis.
pZ	direction 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

pQua	the quaternion you want to extract

Returns: the Rotation matrix extracted from the Quaternion

Rotation AL::Math::rotationFromTransform ( const Transform & pTransform )

Parameters

pTransform position 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

pTransform	the given transform
pRotation	a Rotation to be set with the reslut

Transform AL::Math::transformFromPose2D ( const Pose2D & pPose )

Create a Transform from a Pose2D.

Parameters

pPose the 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

pPose	the Pose2D to extract
pT	the 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

pPosition position 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

pPosition	a Position3D cartesian coordinates
pTransform	the given Transform

Transform AL::Math::transformFromPosition6D ( const Position6D & pPosition6D )

Create a Transform from a Position6D.

Parameters

pPosition6D the 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

pRotation the 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

pRotation the 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

pRotation	the given Rotation
pTransform	the 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

pRot	the given Rotation
pX	the translation along x axis
pY	the translation along y axis
pZ	the 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

pPos	the given Position3D
pRot	the given Rotation

Returns: the Transform result.

Transform AL::Math::transformFromRotVec	(	const int	pAxis,
		const float	pTheta,
		const Position3D &	pPos
	)

Parameters

pAxis	the Rotation you want to extract
pTheta	the Rotation you want to extract
pPos	the Rotation you want to extract

Returns: the result Transform

Transform AL::Math::transformFromRotVec ( const Position3D & pPos )

Parameters

pPos	the Rotation you want to extract

Returns: the result Transform

Transform AL::Math::transformFromRotVec	(	const int &	pAxis,
		const float &	pTheta
	)

Parameters

pAxis	the Rotation you want to extract
pTheta	the 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

pAxis	the Rotation you want to extract
pTheta	the rotation you want to extract
pPos	the Position3D you want to extract
pT	the Rotation you want to extract

void AL::Math::transformFromRotVecInPlace	(	const Position3D &	pPos,
		Transform &	pT
	)

Parameters

pPos	the Rotation you want to extract
pT	the 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

pT	the 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

pT	the given Transform
pVel	the 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

pTIn1	cartesian coordinates
pTIn2	the given Transform
pVal	the 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

pTIn1	the first given Transform
pTIn2	the second given Transform
pVal	the value between 0 and 1 used in the logarithmic mean
pTOut	the output Transform.

Transform AL::Math::velocityExponential ( const Velocity6D & pVel )

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

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

Parameters

pVel	the 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

pAngles	the input/output angles
pWeights	the input/output weights

Returns: The computed mean (in ]-PI, PI]).

Functions

Detailed Description

Function Documentation