Functions
std::vector< Pose2D >	AL::Math::getDubinsSolutions (const Pose2D &pTargetPose, const float pCircleRadius)
	Get the dubins solutions.
bool	AL::Math::clipData (const float &pMin, const float &pMax, float &pData)
	Clip an input data inside min and max limit.
Position6D	AL::Math::position6DFromVelocity6D (const Velocity6D &pVel)
	Create a Position6D from a Velocity6D.
Position3D	AL::Math::operator* (const Rotation &pRot, const Position3D &pPos)
	Overloading of operator * between Rotation and Position3D:
Velocity6D	AL::Math::operator* (const float pVal, const Position6D &pPos)
	Overloading of operator * for float to Position6D, give a Velocity6D:
Rotation	AL::Math::rotationFromAngleDirection (const float &pTheta, const Position3D &pPos)
	Creates a 3*3 Rotation Matrix from a an angle and a normalized Position3D.
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]$ .
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]$ .
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]$ .
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.
Transform	AL::Math::transformMean (const Transform &pTIn1, const Transform &pTIn2, const float &pVal=0.5f)
	Preform a logarithmic mean of pTIn1 and pTIn2.
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.
Transform	AL::Math::transformFromRotationPosition3D (const Rotation &pRot, const Position3D &pPos)
	Create a Transform from a Position3D and a Rotation.
void	AL::Math::transformFromPosition3DInPlace (const Position3D &pPosition, Transform &pTransform)
	Modify pTransform to set the translation part to pPosition.
Transform	AL::Math::transformFromPosition3D (const Position3D &pPosition)
	Create a 4*4 transform matrix from cartesian coordinates given in pPosition.
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.
Transform	AL::Math::transformFromRotation (const Rotation &pRotation)
void	AL::Math::rotationFromTransformInPlace (const Transform &pTransform, Rotation &pRotation)
	Extract the position coordinates from a Transform.
Rotation	AL::Math::rotationFromTransform (const Transform &pTransform)
Rotation3D	AL::Math::rotation3DFromRotation (const Rotation &pRotation)
void	AL::Math::position6DFromTransformInPlace (const Transform &pT, Position6D &pPos)
	Compute Position6D corresponding to the Transform.
Position6D	AL::Math::position6DFromTransform (const Transform &pT)
	Compute Position6D corresponding to 4*4 Homogenous Transform.
void	AL::Math::transformFromPose2DInPlace (const Pose2D &pPose, Transform &pT)
	Compute a Transform from a Pose2D.
Transform	AL::Math::transformFromPose2D (const Pose2D &pPose)
	Create a Transform from a Pose2D.
void	AL::Math::pose2DFromTransformInPlace (const Transform &pT, Pose2D &pPos)
	Compute a Pose2D from a Transform.
Pose2D	AL::Math::pose2DFromTransform (const Transform &pT)
	Create a Pose2D from a Transform.
Transform	AL::Math::transformFromRotation3D (const Rotation3D &pRotation)
	Create a Transform from a Rotation3D.
Transform	AL::Math::transformFromPosition6D (const Position6D &pPosition6D)
	Create a Transform from a Position6D.
void	AL::Math::position6DFromTransformDiffInPlace (const Transform &pCurrent, const Transform &pTarget, Position6D &result)
	Computes a 6 differential motion require to move from a 44 Homogenous transform matrix Current to a 44 Homogenous transform matrix target.
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.
void	AL::Math::position3DFromTransformInPlace (const Transform &pT, Position3D &pPos)
	Compute a Position3D from a Transform.
Position3D	AL::Math::position3DFromTransform (const Transform &pT)
	Create a Position3D from a Transform.
Rotation3D	AL::Math::rotation3DFromTransform (const Transform &pT)
	Create a Rotation3D (Roll, Pitch, Yaw) corresponding to the rotational part of the Transform.
void	AL::Math::transformFromRotVecInPlace (const int pAxis, const float pTheta, const Position3D &pPos, Transform &pT)
	Compute a Transform from.
Transform	AL::Math::transformFromRotVec (const int pAxis, const float pTheta, const Position3D &pPos)
	Compute a Transform from a Rotation3D.
void	AL::Math::transformFromRotVecInPlace (const Position3D &pPos, Transform &pT)
	Compute a Transform from a Rotation3D.
Transform	AL::Math::transformFromRotVec (const Position3D &pPos)
	Compute a Transform from a Rotation3D.
Transform	AL::Math::transformFromRotVec (const int &pAxis, const float &pTheta)
	Compute a Transform from a Rotation3D.
const bool	AL::Math::avoidFootCollision (const std::vector< Pose2D > &pLFootBoundingBox, const std::vector< Pose2D > &pRFootBoundingBox, const bool &pIsLeftSupport, Pose2D &pMove)
	Compute the best position(orientation) of the foot to avoid collision.
const bool	AL::Math::clipFootWithEllipse (const float &pMaxFootX, const float &pMaxFootY, Pose2D &pMove)
	Clip foot move with ellipsoid function.

Function Documentation

const bool AL::Math::avoidFootCollision	(	const std::vector< Pose2D > &	pLFootBoundingBox,
		const std::vector< Pose2D > &	pRFootBoundingBox,
		const bool &	pIsLeftSupport,
		Pose2D &	pMove
	)

Compute the best position(orientation) of the foot to avoid collision.

Parameters:

pLFootBoundingBox	vector<Pose2D> of the left footBoundingBox.
pRFootBoundingBox	vector<Pose2D> 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::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.

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.

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.

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

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

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::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 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
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.

Rotation3D AL::Math::rotation3DFromRotation ( const Rotation & pRotation )

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.

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::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 a Rotation3D.

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
	)

Compute a Transform from a Rotation3D.

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 )

Compute a Transform from a Rotation3D.

Parameters:

pPos	the Rotation you want to extract

Returns:: the result Transform

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

Compute a Transform from a Rotation3D.

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
	)

Compute a Transform from a Rotation3D.

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

Functions

Function Documentation