Functions
ALMATH_API std::vector< Pose2D >	AL::Math::getDubinsSolutions (const Pose2D &pTargetPose, const float pCircleRadius)
	Get the dubins solutions. More...

ALMATH_API void	AL::Math::modulo2PIInPlace (float &pAngle)
	Set an angle between ]-PI, PI]. More...

ALMATH_API float	AL::Math::modulo2PI (float pAngle)
	Return an angle between ]-PI, PI]. More...

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

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

ALMATH_API bool	AL::Math::clipData (const float &pMin, const float &pMax, float &pData)
	Clip an input data inside min and max limit. More...

ALMATH_API void	AL::Math::changeReferencePose2D (const float &pTheta, const Pose2D &pPosIn, Pose2D &pPosOut)

ALMATH_API void	AL::Math::changeReferencePose2DInPlace (const float &pTheta, Pose2D &pPosOut)
	Change orientation of a Pose2D in place. More...

ALMATH_API Position6D	AL::Math::position6DFromVelocity6D (const Velocity6D &pVel)
	Create a Position6D from a Velocity6D More...

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

ALMATH_API Position2D	AL::Math::position2DFromPose2D (const Pose2D &pPose2D)
	Create a Position2D from a Pose2D More...

ALMATH_API Position3D	AL::Math::position3DFromPosition6D (const Position6D &pPosition6D)
	Create a Position3D from a Position6D More...

ALMATH_API Position3D	AL::Math::operator* (const Rotation &pRot, const Position3D &pPos)
	Overloading of operator * between Rotation and Position3D: More...

ALMATH_API Position3D	AL::Math::operator* (const Quaternion &pQuat, const Position3D &pPos)
	Overloading of operator * between Quaternion and Position3D More...

ALMATH_API Velocity6D	AL::Math::operator* (const float pVal, const Position6D &pPos)
	Overloading of operator * for float to Position6D, give a Velocity6D: More...

ALMATH_API Velocity3D	AL::Math::operator* (const float pVal, const Position3D &pPos)
	Overloading of operator * for float to Position3D, give a Velocity3D: More...

ALMATH_API Velocity3D	AL::Math::operator* (const Rotation &pRot, const Velocity3D &pVel)
	Overloading of operator * between Rotation and Velocity3D: More...

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

ALMATH_API void	AL::Math::position6DFromPose2DInPlace (const Pose2D &pPose2D, Position6D &pPosition6D)
	Compute a Position6D from a Pose2D. More...

ALMATH_API Position6D	AL::Math::position6DFromPose2D (const Pose2D &pPose2D)
	Create a Position6D from a Pose2D. More...

ALMATH_API void	AL::Math::position6DFromPosition3DInPlace (const Position3D &pPosition3D, Position6D &pPosition6D)
	Compute a Position6D from a Position3D. More...

ALMATH_API Position6D	AL::Math::position6DFromPosition3D (const Position3D &pPosition3D)
	Create a Position6D from a Position3D. More...

ALMATH_API void	AL::Math::pose2DFromPosition6DInPlace (const Position6D &pPosition6D, Pose2D &pPose2D)
	Compute a Pose2D from a Position6D. More...

ALMATH_API Pose2D	AL::Math::pose2DFromPosition6D (const Position6D &pPosition6D)
	Create a Pose2D from a Position6D. More...

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

ALMATH_API Pose2D	AL::Math::pose2DFromPosition2D (const Position2D &pPosition2D, const float pAngle=0.0f)
	Create a Pose2D from a Position2D. More...

ALMATH_API Position2D	AL::Math::operator* (const Pose2D &pVal, const Position2D &pPos)
	Overloading of operator * for Pose2D to Position2D, give a Position2D: More...

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

ALMATH_API void	AL::Math::rotationFromQuaternion (const Quaternion &pQua, Rotation &pRot)
	Create a Rotation Matrix from a Quaternion More...

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

ALMATH_API void	AL::Math::quaternionPosition3DFromPosition6D (const Position6D &pPos6D, Quaternion &pQua, Position3D &pPos3D)
	Convert a Position6D to Quaternion and Position3D More...

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

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

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

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

ALMATH_API void	AL::Math::changeReferenceVelocity6D (const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)

ALMATH_API void	AL::Math::changeReferencePosition6D (const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)

ALMATH_API void	AL::Math::changeReferencePosition3D (const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)

ALMATH_API void	AL::Math::changeReferenceTransposePosition3D (const Transform &pT, const Position3D &pPosIn, Position3D &pPosOut)

ALMATH_API void	AL::Math::changeReferenceTransform (const Transform &pT, const Transform &pTIn, Transform &pTOut)

ALMATH_API void	AL::Math::changeReferenceTransposeTransform (const Transform &pT, const Transform &pTIn, Transform &pTOut)

ALMATH_API void	AL::Math::changeReferenceTransposeVelocity6D (const Transform &pT, const Velocity6D &pVelIn, Velocity6D &pVelOut)

ALMATH_API void	AL::Math::changeReferenceTransposePosition6D (const Transform &pT, const Position6D &pPosIn, Position6D &pPosOut)

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

ALMATH_API Transform	AL::Math::transformMean (const Transform &pTIn1, const Transform &pTIn2, const float &pVal=0.5f)
	Preform a logarithmic mean of pTIn1 and pTIn2. More...

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

ALMATH_API Transform	AL::Math::transformFromRotationPosition3D (const Rotation &pRot, const Position3D &pPos)
	Create a Transform from a Position3D and a Rotation. More...

ALMATH_API void	AL::Math::transformFromPosition3DInPlace (const Position3D &pPosition, Transform &pTransform)
	Modify pTransform to set the translation part to pPosition. More...

ALMATH_API Transform	AL::Math::transformFromPosition3D (const Position3D &pPosition)
	Create a 4*4 transform matrix from cartesian coordinates given in pPosition. More...

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

ALMATH_API Transform	AL::Math::transformFromRotation (const Rotation &pRotation)

ALMATH_API void	AL::Math::rotationFromTransformInPlace (const Transform &pTransform, Rotation &pRotation)
	Extract the position coordinates from a Transform. More...

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

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

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

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

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

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

ALMATH_API void	AL::Math::position6DFromTransformInPlace (const Transform &pT, Position6D &pPos)
	Compute Position6D corresponding to the Transform. More...

ALMATH_API Position6D	AL::Math::position6DFromTransform (const Transform &pT)
	Compute Position6D corresponding to 4*4 Homogenous Transform. More...

ALMATH_API void	AL::Math::transformFromPose2DInPlace (const Pose2D &pPose, Transform &pT)
	Compute a Transform from a Pose2D. More...

ALMATH_API Transform	AL::Math::transformFromPose2D (const Pose2D &pPose)
	Create a Transform from a Pose2D. More...

ALMATH_API void	AL::Math::pose2DFromTransformInPlace (const Transform &pT, Pose2D &pPos)
	Compute a Pose2D from a Transform. More...

ALMATH_API Pose2D	AL::Math::pose2DFromTransform (const Transform &pT)
	Create a Pose2D from a Transform. More...

ALMATH_API void	AL::Math::position2DFromTransformInPlace (const Transform &pT, Position2D &pPos)
	Compute a Position2D from a Transform. More...

ALMATH_API Position2D	AL::Math::position2DFromTransform (const Transform &pT)
	Create a Position2D from a Transform. More...

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

ALMATH_API Transform	AL::Math::transformFromPosition6D (const Position6D &pPosition6D)
	Create a Transform from a Position6D. More...

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

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

ALMATH_API void	AL::Math::position3DFromTransformInPlace (const Transform &pT, Position3D &pPos)
	Compute a Position3D from a Transform. More...

ALMATH_API Position3D	AL::Math::position3DFromTransform (const Transform &pT)
	Create a Position3D from a Transform. More...

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

ALMATH_API void	AL::Math::transformFromRotVecInPlace (const int pAxis, const float pTheta, const Position3D &pPos, Transform &pT)
	Compute a Transform from More...

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

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

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

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

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

ALMATH_API 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

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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

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

ALMATH_API 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

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

ALMATH_API 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]).

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

Return an angle between ]-PI, PI].

Parameters

pAngle the input angle

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

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

Set an angle between ]-PI, PI].

Parameters

pAngle the input/output angle

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

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

ALMATH_API 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

ALMATH_API 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

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

ALMATH_API 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

ALMATH_API 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>());

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

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

Parameters

pTransform position in cartesian coordinates

Returns: the Rotation extracted from the Transform

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

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

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

ALMATH_API 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

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

Parameters

pPos	the Rotation you want to extract

Returns: the result Transform

ALMATH_API 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

ALMATH_API 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

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

Parameters

pPos	the Rotation you want to extract
pT	the Rotation you want to extract

ALMATH_API 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

ALMATH_API 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

ALMATH_API 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

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

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

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

ALMATH_API 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