Data Structures
struct	BaseLine

struct	ContactPoint

struct	ConvexHull2D

struct	ConvexHull3D

struct	ConvexHull6D

struct	OOBB

struct	Plane

struct	Quaternion

struct	Segment

struct	Segment2D

struct	SphericalCoord

struct	TriangleFace

struct	TriangleFace6D

Typedefs
typedef BaseLine< Eigen::Vector3f >	Line

typedef BaseLine< Eigen::Vector2f >	Line2D

typedef std::shared_ptr< ConvexHull2D >	ConvexHull2DPtr

typedef std::shared_ptr< ConvexHull3D >	ConvexHull3DPtr

typedef std::shared_ptr< ConvexHull6D >	ConvexHull6DPtr

Enumerations
enum	IntersectionResult { eParallel, eNoIntersection, eIntersection }

Functions
Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	getRotation (const Eigen::Vector3f &from, const Eigen::Vector3f &to)

float VIRTUAL_ROBOT_IMPORT_EXPORT	getAngle (const Quaternion &q)
	get the corresponding angle of rotation that is defined by the quaternion (radian) More...

Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	getDelta (const Quaternion &q1, const Quaternion &q2)
	Return the quaternion that defines the difference between the two given rotations. More...

Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	getInverse (const Quaternion &q)
	Return the inverse quaternion. More...

Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	getConjugated (const Quaternion &q)
	Return the conjugated quaternion. More...

Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	multiplyQuaternions (const Quaternion &q1, const Quaternion &q2)
	Returns q1*q2. More...

float VIRTUAL_ROBOT_IMPORT_EXPORT	getDot (const Quaternion &q1, const Quaternion &q2)
	returns q1 dot q2 More...

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	getMean (std::vector< MathTools::Quaternion > quaternions)
	Computes mean orientation of quaternions. More...

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	slerp (const MathTools::Quaternion &q1, const MathTools::Quaternion &q2, float alpha)

float VIRTUAL_ROBOT_IMPORT_EXPORT	fmod (float value, float boundLow, float boundHigh)

float VIRTUAL_ROBOT_IMPORT_EXPORT	angleMod2PI (float value)

float VIRTUAL_ROBOT_IMPORT_EXPORT	angleModPI (float value)

float VIRTUAL_ROBOT_IMPORT_EXPORT	angleModX (float value, float center)

float VIRTUAL_ROBOT_IMPORT_EXPORT	Lerp (float a, float b, float f)

float VIRTUAL_ROBOT_IMPORT_EXPORT	ILerp (float a, float b, float f)

float VIRTUAL_ROBOT_IMPORT_EXPORT	AngleLerp (float a, float b, float f)

float VIRTUAL_ROBOT_IMPORT_EXPORT	AngleDelta (float angle1, float angle2)

SphericalCoord VIRTUAL_ROBOT_IMPORT_EXPORT	toSphericalCoords (const Eigen::Vector3f &pos)

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	toPosition (const SphericalCoord &sc)

void VIRTUAL_ROBOT_IMPORT_EXPORT	rpy2eigen4f (float r, float p, float y, Eigen::Matrix4f &m)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	rpy2eigen4f (float r, float p, float y)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	rpy2eigen4f (const Eigen::Vector3f &rpy)

Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT	rpy2eigen3f (float r, float p, float y)

Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT	rpy2eigen3f (const Eigen::Vector3f &rpy)

void VIRTUAL_ROBOT_IMPORT_EXPORT	posrpy2eigen4f (const float x[6], Eigen::Matrix4f &m)

void VIRTUAL_ROBOT_IMPORT_EXPORT	posrpy2eigen4f (const Eigen::Vector3f &pos, const Eigen::Vector3f &rpy, Eigen::Matrix4f &m)

void VIRTUAL_ROBOT_IMPORT_EXPORT	posrpy2eigen4f (float x, float y, float z, float roll, float pitch, float yaw, Eigen::Matrix4f &m)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	posrpy2eigen4f (const Eigen::Vector3f &pos, const Eigen::Vector3f &rpy)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	posrpy2eigen4f (float x, float y, float z, float roll, float pitch, float yaw)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	posquat2eigen4f (float x, float y, float z, float qx, float qy, float qz, float qw)

Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT	quat2eigen3f (float qx, float qy, float qz, float qw)

void VIRTUAL_ROBOT_IMPORT_EXPORT	eigen4f2rpy (const Eigen::Matrix4f &m, float x[6])

void VIRTUAL_ROBOT_IMPORT_EXPORT	eigen4f2rpy (const Eigen::Matrix4f &m, Eigen::Vector3f &storeRPY)

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	eigen4f2rpy (const Eigen::Matrix4f &m)

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	eigen3f2rpy (const Eigen::Matrix3f &m)

Eigen::Matrix< float, 6, 1 > VIRTUAL_ROBOT_IMPORT_EXPORT	eigen4f2posrpy (const Eigen::Matrix4f &m)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	quat2eigen4f (float x, float y, float z, float w)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	quat2eigen4f (const Quaternion q)

void VIRTUAL_ROBOT_IMPORT_EXPORT	quat2eigen4f (float x, float y, float z, float w, Eigen::Matrix4f &m)

void VIRTUAL_ROBOT_IMPORT_EXPORT	quat2eigen4f (const Quaternion q, Eigen::Matrix4f &m)

Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	eigen4f2quat (const Eigen::Matrix4f &m)

void VIRTUAL_ROBOT_IMPORT_EXPORT	eigen4f2quat (const Eigen::Matrix4f &m, Quaternion &q)

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	getTranslation (const Eigen::Matrix4f &m)

void VIRTUAL_ROBOT_IMPORT_EXPORT	getTranslation (const Eigen::Matrix4f &m, Eigen::Vector3f &v)

void VIRTUAL_ROBOT_IMPORT_EXPORT	eigen4f2axisangle (const Eigen::Matrix4f &m, Eigen::Vector3f &storeAxis, float &storeAngle)

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT	axisangle2eigen4f (const Eigen::Vector3f &axis, float angle)

void VIRTUAL_ROBOT_IMPORT_EXPORT	axisangle2eigen4f (const Eigen::Vector3f &axis, float angle, Eigen::Matrix4f &storeResult)

Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT	axisangle2eigen3f (const Eigen::Vector3f &axis, float angle)

Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	axisangle2quat (const Eigen::Vector3f &axis, float angle)

void VIRTUAL_ROBOT_IMPORT_EXPORT	getDelta (const Eigen::Matrix4f &m1, const Eigen::Matrix4f &m2, float &storeDetalPos, float &storeDeltaRot)

float VIRTUAL_ROBOT_IMPORT_EXPORT	rad2deg (float rad)

float VIRTUAL_ROBOT_IMPORT_EXPORT	deg2rad (float deg)

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	quat2hopf (const Quaternion &q)

Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT	hopf2quat (const Eigen::Vector3f &hopf)

Plane VIRTUAL_ROBOT_IMPORT_EXPORT	getFloorPlane ()
	Create a floor plane. More...

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	projectPointToPlane (const Eigen::Vector3f &point, const Plane &plane)
	Get the projected point in 3D. More...

Line VIRTUAL_ROBOT_IMPORT_EXPORT	intersectPlanes (const Plane &p1, const Plane &p2)
	Get the intersection line of two planes. If planes are parallel, the resulting line is invalid, i.e. has a zero direction vector! More...

IntersectionResult VIRTUAL_ROBOT_IMPORT_EXPORT	intersectSegmentPlane (const Segment &segment, const Plane &plane, Eigen::Vector3f &storeResult)

IntersectionResult VIRTUAL_ROBOT_IMPORT_EXPORT	intersectOOBBPlane (const OOBB &oobb, const Plane &plane, std::vector< Eigen::Vector3f > &storeResult)

template<typename VectorT >
VectorT	nearestPointOnLine (const BaseLine< VectorT > &l, const VectorT &p)
	Returns nearest point to p on line l. More...

template<typename VectorT >
VectorT	nearestPointOnSegment (const VectorT &firstEndPoint, const VectorT &secondEndPoint, const VectorT &p)
	Returns nearest point to p on segment given by the two end points. More...

template<typename VectorT >
float	distPointLine (const BaseLine< VectorT > &l, const VectorT &p)
	Returns the distance of vector p to line l. More...

template<typename VectorT >
float	distPointSegment (const VectorT &firstEndPoint, const VectorT &secondEndPoint, const VectorT &p)
	Returns the distance of vector p to segment given by the two end points. More...

bool VIRTUAL_ROBOT_IMPORT_EXPORT	collinear (const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3)
	Check if three points are collinear. More...

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	findNormal (const std::vector< Eigen::Vector3f > &points)

Eigen::Vector2f VIRTUAL_ROBOT_IMPORT_EXPORT	projectPointToPlane2D (const Eigen::Vector3f &point, const Plane &plane)
	Get the projected point in 2D (local coordinate system of the plane) More...

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	planePoint3D (const Eigen::Vector2f &pointLocal, const Plane &plane)
	Get the corresponding point in 3D. More...

float VIRTUAL_ROBOT_IMPORT_EXPORT	getDistancePointPlane (const Eigen::Vector3f &point, const Plane &plane)

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	transformPosition (const Eigen::Vector3f &pos, const Eigen::Matrix4f &m)

Eigen::Vector2f VIRTUAL_ROBOT_IMPORT_EXPORT	transformPosition (const Eigen::Vector2f &pos, const Eigen::Matrix4f &m)

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT	randomPointInTriangle (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const Eigen::Vector3f &v3)
	Get a random point inside the triangle that is spanned by v1, v2 and v3. More...

bool VIRTUAL_ROBOT_IMPORT_EXPORT	onNormalPointingSide (const Eigen::Vector3f &point, const Plane &p)

float VIRTUAL_ROBOT_IMPORT_EXPORT	getAngle (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2)

float VIRTUAL_ROBOT_IMPORT_EXPORT	getCartesianPoseDiff (const Eigen::Matrix4f &p1, const Eigen::Matrix4f &p2, float rotInfluence=3.0f)

void VIRTUAL_ROBOT_IMPORT_EXPORT	getPoseDiff (const Eigen::Matrix4f &p1, const Eigen::Matrix4f &p2, float &storePosDiff, float &storeRotDiffRad)
	getPoseDiff Computes the translational and rotational difference between two poses. More...

float VIRTUAL_ROBOT_IMPORT_EXPORT	getTriangleArea (const Eigen::Vector3f &a, const Eigen::Vector3f &b, const Eigen::Vector3f &c)

float	isLeft (Eigen::Vector2f P0, Eigen::Vector2f P1, Eigen::Vector2f P2)

ConvexHull2DPtr VIRTUAL_ROBOT_IMPORT_EXPORT	createConvexHull2D (const std::vector< Eigen::Vector2f > &points)

Eigen::Vector2f VIRTUAL_ROBOT_IMPORT_EXPORT	getConvexHullCenter (ConvexHull2DPtr ch)

bool VIRTUAL_ROBOT_IMPORT_EXPORT	isInside (const Eigen::Vector2f &p, ConvexHull2DPtr hull)

std::vector< Eigen::Vector2f >	sortPoints (const std::vector< Eigen::Vector2f > &points)

bool VIRTUAL_ROBOT_IMPORT_EXPORT	ensureOrthonormalBasis (Eigen::Vector3f &x, Eigen::Vector3f &y, Eigen::Vector3f &z)

bool VIRTUAL_ROBOT_IMPORT_EXPORT	GramSchmidt (std::vector< Eigen::VectorXf > &basis)
	perform the Gram-Schmidt method for building an orthonormal basis More...

bool VIRTUAL_ROBOT_IMPORT_EXPORT	randomLinearlyIndependentVector (const std::vector< Eigen::VectorXf > basis, Eigen::VectorXf &storeResult)
	Searches a vector that is linearly independent of the given basis. More...

bool VIRTUAL_ROBOT_IMPORT_EXPORT	containsVector (const Eigen::MatrixXf &m, const Eigen::VectorXf &v)
	Checks if m contains a col vector equal to v (distance up to 1e-8 are considered as equal) More...

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT	getBasisTransformation (const std::vector< Eigen::VectorXf > &basisSrc, const std::vector< Eigen::VectorXf > &basisDst)

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT	getBasisTransformation (const Eigen::MatrixXf &basisSrc, const Eigen::MatrixXf &basisDst)

Eigen::VectorXf VIRTUAL_ROBOT_IMPORT_EXPORT	getPermutation (const Eigen::VectorXf &inputA, const Eigen::VectorXf &inputB, unsigned int i)

int VIRTUAL_ROBOT_IMPORT_EXPORT	pow_int (int a, int b)

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT	getPseudoInverse (const Eigen::MatrixXf &m, float tol=1e-5f)

Eigen::MatrixXd VIRTUAL_ROBOT_IMPORT_EXPORT	getPseudoInverseD (const Eigen::MatrixXd &m, double tol=1e-5)

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT	getPseudoInverseDamped (const Eigen::MatrixXf &m, float lambda=1.0f)

Eigen::MatrixXd VIRTUAL_ROBOT_IMPORT_EXPORT	getPseudoInverseDampedD (const Eigen::MatrixXd &m, double lambda=1.0)

double VIRTUAL_ROBOT_IMPORT_EXPORT	getDamping (const Eigen::MatrixXd &matrix)

double VIRTUAL_ROBOT_IMPORT_EXPORT	getDamping (const Eigen::MatrixXf &matrix)

template<typename T >
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > VIRTUAL_ROBOT_IMPORT_EXPORT	getDampedLeastSquareInverse (const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > &jacobian, double squaredDampingFactor=0.01f)
	Calculates the damped least square inverse by J^T * (JJ^T + k^2 I)^{-1} Jacobian J Damping factor k^2. More...

bool VIRTUAL_ROBOT_IMPORT_EXPORT	isValid (const Eigen::MatrixXf &v)

void VIRTUAL_ROBOT_IMPORT_EXPORT	print (const ContactPoint &p)

void VIRTUAL_ROBOT_IMPORT_EXPORT	print (const std::vector< ContactPoint > &points)

void VIRTUAL_ROBOT_IMPORT_EXPORT	print (const Eigen::VectorXf &v, bool endline=true)

void VIRTUAL_ROBOT_IMPORT_EXPORT	printMat (const Eigen::MatrixXf &m, bool endline=true)

void VIRTUAL_ROBOT_IMPORT_EXPORT	print (const std::vector< float > &v, bool endline=true)

std::string VIRTUAL_ROBOT_IMPORT_EXPORT	getTransformXMLString (const Eigen::Matrix4f &m, int tabs, bool skipMatrixTag=false)

std::string VIRTUAL_ROBOT_IMPORT_EXPORT	getTransformXMLString (const Eigen::Matrix4f &m, const std::string &tabs, bool skipMatrixTag=false)

std::string VIRTUAL_ROBOT_IMPORT_EXPORT	getTransformXMLString (const Eigen::Matrix3f &m, int tabs, bool skipMatrixTag=false)

std::string VIRTUAL_ROBOT_IMPORT_EXPORT	getTransformXMLString (const Eigen::Matrix3f &m, const std::string &tabs, bool skipMatrixTag=false)

void VIRTUAL_ROBOT_IMPORT_EXPORT	convertMM2M (const std::vector< ContactPoint > points, std::vector< ContactPoint > &storeResult)

Typedef Documentation

◆ ConvexHull2DPtr

typedef std::shared_ptr< ConvexHull2D > VirtualRobot::MathTools::ConvexHull2DPtr

◆ ConvexHull3DPtr

typedef std::shared_ptr< ConvexHull3D > VirtualRobot::MathTools::ConvexHull3DPtr

◆ ConvexHull6DPtr

typedef std::shared_ptr< ConvexHull6D > VirtualRobot::MathTools::ConvexHull6DPtr

◆ Line

typedef BaseLine< Eigen::Vector3f > VirtualRobot::MathTools::Line

◆ Line2D

typedef BaseLine< Eigen::Vector2f > VirtualRobot::MathTools::Line2D

Enumeration Type Documentation

◆ IntersectionResult

enum VirtualRobot::MathTools::IntersectionResult

Enumerator
eParallel
eNoIntersection
eIntersection

Function Documentation

◆ AngleDelta()

float VirtualRobot::MathTools::AngleDelta	(	float	angle1,
		float	angle2
	)

◆ AngleLerp()

float VirtualRobot::MathTools::AngleLerp	(	float	a,
		float	b,
		float	f
	)

◆ angleMod2PI()

float VirtualRobot::MathTools::angleMod2PI ( float value )

◆ angleModPI()

float VirtualRobot::MathTools::angleModPI ( float value )

◆ angleModX()

float VirtualRobot::MathTools::angleModX	(	float	value,
		float	center
	)

◆ axisangle2eigen3f()

Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2eigen3f	(	const Eigen::Vector3f &	axis,
		float	angle
	)

◆ axisangle2eigen4f() [1/2]

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2eigen4f	(	const Eigen::Vector3f &	axis,
		float	angle
	)

◆ axisangle2eigen4f() [2/2]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2eigen4f	(	const Eigen::Vector3f &	axis,
		float	angle,
		Eigen::Matrix4f &	storeResult
	)

◆ axisangle2quat()

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2quat	(	const Eigen::Vector3f &	axis,
		float	angle
	)

◆ collinear()

bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::collinear	(	const Eigen::Vector3f &	p1,
		const Eigen::Vector3f &	p2,
		const Eigen::Vector3f &	p3
	)

Check if three points are collinear.

◆ containsVector()

bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::containsVector	(	const Eigen::MatrixXf &	m,
		const Eigen::VectorXf &	v
	)

Checks if m contains a col vector equal to v (distance up to 1e-8 are considered as equal)

◆ convertMM2M()

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::convertMM2M	(	const std::vector< ContactPoint >	points,
		std::vector< ContactPoint > &	storeResult
	)

◆ createConvexHull2D()

MathTools::ConvexHull2DPtr VirtualRobot::MathTools::createConvexHull2D ( const std::vector< Eigen::Vector2f > & points )

◆ deg2rad()

float VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::deg2rad ( float deg )

◆ distPointLine()

template<typename VectorT >

float VirtualRobot::MathTools::distPointLine	(	const BaseLine< VectorT > &	l,
		const VectorT &	p
	)

inline

Returns the distance of vector p to line l.

◆ distPointSegment()

template<typename VectorT >

float VirtualRobot::MathTools::distPointSegment	(	const VectorT &	firstEndPoint,
		const VectorT &	secondEndPoint,
		const VectorT &	p
	)

inline

Returns the distance of vector p to segment given by the two end points.

◆ eigen3f2rpy()

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen3f2rpy ( const Eigen::Matrix3f & m )

◆ eigen4f2axisangle()

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2axisangle	(	const Eigen::Matrix4f &	m,
		Eigen::Vector3f &	storeAxis,
		float &	storeAngle
	)

◆ eigen4f2posrpy()

Eigen::Matrix< float, 6, 1 > VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2posrpy ( const Eigen::Matrix4f & m )

◆ eigen4f2quat() [1/2]

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2quat ( const Eigen::Matrix4f & m )

◆ eigen4f2quat() [2/2]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2quat	(	const Eigen::Matrix4f &	m,
		Quaternion &	q
	)

◆ eigen4f2rpy() [1/3]

void VirtualRobot::MathTools::eigen4f2rpy	(	const Eigen::Matrix4f &	m,
		float	x[6]
	)

Convert homogeneous matrix to translation and rpy rotation.

Parameters

m	The matrix to be converted
x	The result is stored in this float array (x,y,z,roll,pitch,yaw)

◆ eigen4f2rpy() [2/3]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2rpy	(	const Eigen::Matrix4f &	m,
		Eigen::Vector3f &	storeRPY
	)

◆ eigen4f2rpy() [3/3]

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2rpy ( const Eigen::Matrix4f & m )

◆ ensureOrthonormalBasis()

bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::ensureOrthonormalBasis	(	Eigen::Vector3f &	x,
		Eigen::Vector3f &	y,
		Eigen::Vector3f &	z
	)

◆ findNormal()

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::findNormal ( const std::vector< Eigen::Vector3f > & points )

assuming all points to be in a plane. Returns normal of this plane.

◆ fmod()

float VirtualRobot::MathTools::fmod	(	float	value,
		float	boundLow,
		float	boundHigh
	)

◆ getAngle() [1/2]

float VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getAngle ( const Quaternion & q )

get the corresponding angle of rotation that is defined by the quaternion (radian)

◆ getAngle() [2/2]

float VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getAngle	(	const Eigen::Vector3f &	v1,
		const Eigen::Vector3f &	v2
	)

Returns angle between v1 and v2 [rad].

◆ getBasisTransformation() [1/2]

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getBasisTransformation	(	const std::vector< Eigen::VectorXf > &	basisSrc,
		const std::vector< Eigen::VectorXf > &	basisDst
	)

Computes the matrix describing the basis transformation from basis formed by vectors in basisSrc to basisDst. So v_{Src} can be transformed to v_{Dst}, given as linear combination of basisDst vectors, by v_{Dst} = T v_{Src} with T is the BasisTransformationMatrix given by this method.

Parameters

basisSrc	The initial basis vectors.
basisDst	The final basis vectors.

Returns: The transformation matrix T.

◆ getBasisTransformation() [2/2]

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getBasisTransformation	(	const Eigen::MatrixXf &	basisSrc,
		const Eigen::MatrixXf &	basisDst
	)

Computes the matrix describing the basis transformation from basis formed by vectors in basisSrc to basisDst. So v_{Src} can be transformed to v_{Dst}, given as linear combination of basisDst vectors, by v_{Dst} = T v_{Src} with T is the BasisTransformationMatrix given by this method.

Parameters

basisSrc	The column vectors are the initial basis vectors
basisDst	The column vectors are the final basis vectors.

Returns: The transformation matrix T.

◆ getCartesianPoseDiff()

float VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getCartesianPoseDiff	(	const Eigen::Matrix4f &	p1,
		const Eigen::Matrix4f &	p2,
		float	rotInfluence = `3.0f`
	)

This method unites the translational and rotational difference of two Cartesian poses to one metric value. The translational distance is the norm in mm (counts 1) The rotational distance is the approximated angle between the two orientations in degrees (counts 3, or whatever you specify with rotInfluence) In the standard setting -> 3mm equals 1 degree in this metric The angle is approximated by replacing the costly eq alpha=2*acos(q0) with a linear term: alpha = 180-(q0+1)*90

◆ getConjugated()

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getConjugated ( const Quaternion & q )

Return the conjugated quaternion.

◆ getConvexHullCenter()

Eigen::Vector2f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getConvexHullCenter ( ConvexHull2DPtr ch )

◆ getDampedLeastSquareInverse()

template<typename T >

Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getDampedLeastSquareInverse	(	const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > &	jacobian,
		double	squaredDampingFactor = `0.01f`
	)

Calculates the damped least square inverse by J^T * (JJ^T + k^2 I)^{-1} Jacobian J Damping factor k^2.

◆ getDamping() [1/2]

double VirtualRobot::MathTools::getDamping ( const Eigen::MatrixXd & matrix )

Calculates damping factor for singularity avoidance

◆ getDamping() [2/2]

double VirtualRobot::MathTools::getDamping ( const Eigen::MatrixXf & matrix )

◆ getDelta() [1/2]

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getDelta	(	const Quaternion &	q1,
		const Quaternion &	q2
	)

Return the quaternion that defines the difference between the two given rotations.

◆ getDelta() [2/2]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getDelta	(	const Eigen::Matrix4f &	m1,
		const Eigen::Matrix4f &	m2,
		float &	storeDetalPos,
		float &	storeDeltaRot
	)

Compute the delta of two poses.

Parameters

m1	The first pose.
m2	The second pose.
storeDetalPos	The position delta is stored here.
storeDeltaRot	The orientation delta is stored here [radian]

◆ getDistancePointPlane()

float VirtualRobot::MathTools::getDistancePointPlane	(	const Eigen::Vector3f &	point,
		const Plane &	plane
	)

◆ getDot()

float VirtualRobot::MathTools::getDot	(	const Quaternion &	q1,
		const Quaternion &	q2
	)

returns q1 dot q2

◆ getFloorPlane()

MathTools::Plane VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getFloorPlane ( )

Create a floor plane.

◆ getInverse()

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getInverse ( const Quaternion & q )

Return the inverse quaternion.

◆ getMean()

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getMean ( std::vector< MathTools::Quaternion > quaternions )

Computes mean orientation of quaternions.

◆ getPermutation()

Eigen::VectorXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPermutation	(	const Eigen::VectorXf &	inputA,
		const Eigen::VectorXf &	inputB,
		unsigned int	i
	)

◆ getPoseDiff()

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPoseDiff	(	const Eigen::Matrix4f &	p1,
		const Eigen::Matrix4f &	p2,
		float &	storePosDiff,
		float &	storeRotDiffRad
	)

getPoseDiff Computes the translational and rotational difference between two poses.

Parameters

p1	First Pose.
p2	Second Pose.
storePosDiff	The translational absolute distance between p1 and p2 is stored here
storeRotDiffRad	The rotational absolute distance between p1 and p2 is stored here (radian)

Returns

◆ getPseudoInverse()

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverse	(	const Eigen::MatrixXf &	m,
		float	tol = `1e-5f`
	)

Returns the Pseudo inverse matrix.

◆ getPseudoInverseD()

Eigen::MatrixXd VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverseD	(	const Eigen::MatrixXd &	m,
		double	tol = `1e-5`
	)

◆ getPseudoInverseDamped()

Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverseDamped	(	const Eigen::MatrixXf &	m,
		float	lambda = `1.0f`
	)

Returns the damped Pseudo inverse matrix.

◆ getPseudoInverseDampedD()

Eigen::MatrixXd VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverseDampedD	(	const Eigen::MatrixXd &	m,
		double	lambda = `1.0`
	)

◆ getRotation()

MathTools::Quaternion VirtualRobot::MathTools::getRotation	(	const Eigen::Vector3f &	from,
		const Eigen::Vector3f &	to
	)

Return rotation that converts vector from to vector to.

◆ getTransformXMLString() [1/4]

std::string VirtualRobot::MathTools::getTransformXMLString	(	const Eigen::Matrix4f &	m,
		int	tabs,
		bool	skipMatrixTag = `false`
	)

◆ getTransformXMLString() [2/4]

std::string VirtualRobot::MathTools::getTransformXMLString	(	const Eigen::Matrix4f &	m,
		const std::string &	tabs,
		bool	skipMatrixTag = `false`
	)

◆ getTransformXMLString() [3/4]

std::string VirtualRobot::MathTools::getTransformXMLString	(	const Eigen::Matrix3f &	m,
		int	tabs,
		bool	skipMatrixTag = `false`
	)

◆ getTransformXMLString() [4/4]

std::string VirtualRobot::MathTools::getTransformXMLString	(	const Eigen::Matrix3f &	m,
		const std::string &	tabs,
		bool	skipMatrixTag = `false`
	)

◆ getTranslation() [1/2]

Eigen::Vector3f VirtualRobot::MathTools::getTranslation ( const Eigen::Matrix4f & m )

◆ getTranslation() [2/2]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getTranslation	(	const Eigen::Matrix4f &	m,
		Eigen::Vector3f &	v
	)

◆ getTriangleArea()

float VirtualRobot::MathTools::getTriangleArea	(	const Eigen::Vector3f &	a,
		const Eigen::Vector3f &	b,
		const Eigen::Vector3f &	c
	)

◆ GramSchmidt()

bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::GramSchmidt ( std::vector< Eigen::VectorXf > & basis )

perform the Gram-Schmidt method for building an orthonormal basis

◆ hopf2quat()

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::hopf2quat ( const Eigen::Vector3f & hopf )

◆ ILerp()

float VirtualRobot::MathTools::ILerp	(	float	a,
		float	b,
		float	f
	)

◆ intersectOOBBPlane()

MathTools::IntersectionResult VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::intersectOOBBPlane	(	const OOBB &	oobb,
		const Plane &	plane,
		std::vector< Eigen::Vector3f > &	storeResult
	)

Intersect an object oriented bounding box (oobb) with a plane.

Parameters

oobb	The oobb
plane	The plane
storeResult	In case an intersection exists, the intersection area is defined by these points

Returns: If the oobb does not intersect eNoIntersection is returned, otherwise eIntersection.

◆ intersectPlanes()

MathTools::Line VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::intersectPlanes	(	const Plane &	p1,
		const Plane &	p2
	)

Get the intersection line of two planes. If planes are parallel, the resulting line is invalid, i.e. has a zero direction vector!

◆ intersectSegmentPlane()

MathTools::IntersectionResult VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::intersectSegmentPlane	(	const Segment &	segment,
		const Plane &	plane,
		Eigen::Vector3f &	storeResult
	)

Get the intersection of segment and plane.

Parameters

segment	The segment
plane	The plane
storeResult	In case the intersection exists, the result is stored here

Returns: If there is no intersection the result is eNoIntersection. In case the result is eIntersection, the resulting intersection point is stored in storeResult.

◆ isInside()

bool VirtualRobot::MathTools::isInside	(	const Eigen::Vector2f &	p,
		ConvexHull2DPtr	hull
	)

◆ isLeft()

float VirtualRobot::MathTools::isLeft	(	Eigen::Vector2f	P0,
		Eigen::Vector2f	P1,
		Eigen::Vector2f	P2
	)

inline

◆ isValid()

bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::isValid ( const Eigen::MatrixXf & v )

Check if all entries of v are valid numbers (i.e. all entries of v are not NaN and not INF)

◆ Lerp()

float VirtualRobot::MathTools::Lerp	(	float	a,
		float	b,
		float	f
	)

◆ multiplyQuaternions()

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::multiplyQuaternions	(	const Quaternion &	q1,
		const Quaternion &	q2
	)

Returns q1*q2.

◆ nearestPointOnLine()

template<typename VectorT >

VectorT VirtualRobot::MathTools::nearestPointOnLine	(	const BaseLine< VectorT > &	l,
		const VectorT &	p
	)

inline

Returns nearest point to p on line l.

◆ nearestPointOnSegment()

template<typename VectorT >

VectorT VirtualRobot::MathTools::nearestPointOnSegment	(	const VectorT &	firstEndPoint,
		const VectorT &	secondEndPoint,
		const VectorT &	p
	)

inline

Returns nearest point to p on segment given by the two end points.

◆ onNormalPointingSide()

bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::onNormalPointingSide	(	const Eigen::Vector3f &	point,
		const Plane &	p
	)

Returns true, if point is on the side of plane in which the normal vector is pointing.

◆ planePoint3D()

Eigen::Vector3f VirtualRobot::MathTools::planePoint3D	(	const Eigen::Vector2f &	pointLocal,
		const Plane &	plane
	)

Get the corresponding point in 3D.

◆ posquat2eigen4f()

Eigen::Matrix4f VirtualRobot::MathTools::posquat2eigen4f	(	float	x,
		float	y,
		float	z,
		float	qx,
		float	qy,
		float	qz,
		float	qw
	)

◆ posrpy2eigen4f() [1/5]

void VirtualRobot::MathTools::posrpy2eigen4f	(	const float	x[6],
		Eigen::Matrix4f &	m
	)

◆ posrpy2eigen4f() [2/5]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::posrpy2eigen4f	(	const Eigen::Vector3f &	pos,
		const Eigen::Vector3f &	rpy,
		Eigen::Matrix4f &	m
	)

◆ posrpy2eigen4f() [3/5]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::posrpy2eigen4f	(	float	x,
		float	y,
		float	z,
		float	roll,
		float	pitch,
		float	yaw,
		Eigen::Matrix4f &	m
	)

◆ posrpy2eigen4f() [4/5]

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::posrpy2eigen4f	(	const Eigen::Vector3f &	pos,
		const Eigen::Vector3f &	rpy
	)

◆ posrpy2eigen4f() [5/5]

Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::posrpy2eigen4f	(	float	x,
		float	y,
		float	z,
		float	roll,
		float	pitch,
		float	yaw
	)

◆ pow_int()

int VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::pow_int	(	int	a,
		int	b
	)

◆ print() [1/4]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print ( const ContactPoint & p )

◆ print() [2/4]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print ( const std::vector< ContactPoint > & points )

◆ print() [3/4]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print	(	const Eigen::VectorXf &	v,
		bool	endline = `true`
	)

◆ print() [4/4]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print	(	const std::vector< float > &	v,
		bool	endline = `true`
	)

◆ printMat()

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::printMat	(	const Eigen::MatrixXf &	m,
		bool	endline = `true`
	)

◆ projectPointToPlane()

Eigen::Vector3f VirtualRobot::MathTools::projectPointToPlane	(	const Eigen::Vector3f &	point,
		const Plane &	plane
	)

Get the projected point in 3D.

◆ projectPointToPlane2D()

Eigen::Vector2f VirtualRobot::MathTools::projectPointToPlane2D	(	const Eigen::Vector3f &	point,
		const Plane &	plane
	)

Get the projected point in 2D (local coordinate system of the plane)

◆ quat2eigen3f()

Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::quat2eigen3f	(	float	qx,
		float	qy,
		float	qz,
		float	qw
	)

◆ quat2eigen4f() [1/4]

Eigen::Matrix4f VirtualRobot::MathTools::quat2eigen4f	(	float	x,
		float	y,
		float	z,
		float	w
	)

Convert quaternion values to a 3x3 rotation matrix and store it to the rotational component of the result. The translational part of m is zero

Returns: Homogeneous matrix representing the rotation of q.

◆ quat2eigen4f() [2/4]

Eigen::Matrix4f VirtualRobot::MathTools::quat2eigen4f ( const Quaternion q )

◆ quat2eigen4f() [3/4]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::quat2eigen4f	(	float	x,
		float	y,
		float	z,
		float	w,
		Eigen::Matrix4f &	m
	)

◆ quat2eigen4f() [4/4]

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::quat2eigen4f	(	const Quaternion	q,
		Eigen::Matrix4f &	m
	)

◆ quat2hopf()

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::quat2hopf ( const Quaternion & q )

◆ rad2deg()

float VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::rad2deg ( float rad )

◆ randomLinearlyIndependentVector()

bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::randomLinearlyIndependentVector	(	const std::vector< Eigen::VectorXf >	basis,
		Eigen::VectorXf &	storeResult
	)

Searches a vector that is linearly independent of the given basis.

◆ randomPointInTriangle()

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::randomPointInTriangle	(	const Eigen::Vector3f &	v1,
		const Eigen::Vector3f &	v2,
		const Eigen::Vector3f &	v3
	)

Get a random point inside the triangle that is spanned by v1, v2 and v3.

◆ rpy2eigen3f() [1/2]

Eigen::Matrix3f VirtualRobot::MathTools::rpy2eigen3f	(	float	r,
		float	p,
		float	y
	)

◆ rpy2eigen3f() [2/2]

Eigen::Matrix3f VirtualRobot::MathTools::rpy2eigen3f ( const Eigen::Vector3f & rpy )

◆ rpy2eigen4f() [1/3]

void VirtualRobot::MathTools::rpy2eigen4f	(	float	r,
		float	p,
		float	y,
		Eigen::Matrix4f &	m
	)

Convert rpy values to a 3x3 rotation matrix and store it to the rotational component of the given homogeneous matrix. The translation is set to zero.

◆ rpy2eigen4f() [2/3]

Eigen::Matrix4f VirtualRobot::MathTools::rpy2eigen4f	(	float	r,
		float	p,
		float	y
	)

◆ rpy2eigen4f() [3/3]

Eigen::Matrix4f VirtualRobot::MathTools::rpy2eigen4f ( const Eigen::Vector3f & rpy )

◆ slerp()

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::slerp	(	const MathTools::Quaternion &	q1,
		const MathTools::Quaternion &	q2,
		float	alpha
	)

Apply the slerp interpolation

Parameters

q1	The first quaternion
q2	The second quaternion
alpha	A value between 0 and 1

Returns: The intermediate quaternion

◆ sortPoints()

std::vector< Eigen::Vector2f > VirtualRobot::MathTools::sortPoints ( const std::vector< Eigen::Vector2f > & points )

Sort points according to their first coordinate. If two points share the first coord, the second one is used to decide which is "smaller".

◆ toPosition()

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::toPosition ( const SphericalCoord & sc )

◆ toSphericalCoords()

VirtualRobot::MathTools::SphericalCoord VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::toSphericalCoords ( const Eigen::Vector3f & pos )

◆ transformPosition() [1/2]

Eigen::Vector3f VirtualRobot::MathTools::transformPosition	(	const Eigen::Vector3f &	pos,
		const Eigen::Matrix4f &	m
	)

This method can be used to multiply a 3d position with a matrix result = m * pos

◆ transformPosition() [2/2]

Eigen::Vector2f VirtualRobot::MathTools::transformPosition	(	const Eigen::Vector2f &	pos,
		const Eigen::Matrix4f &	m
	)

This method can be used to multiply a 2d position with a matrix result = m * pos

Data Structures

Typedefs

Enumerations

Functions

Typedef Documentation

◆ ConvexHull2DPtr

◆ ConvexHull3DPtr

◆ ConvexHull6DPtr

◆ Line

◆ Line2D

Enumeration Type Documentation

◆ IntersectionResult

Function Documentation

◆ AngleDelta()

◆ AngleLerp()

◆ angleMod2PI()

◆ angleModPI()

◆ angleModX()

◆ axisangle2eigen3f()

◆ axisangle2eigen4f() [1/2]

◆ axisangle2eigen4f() [2/2]

◆ axisangle2quat()

◆ collinear()

◆ containsVector()

◆ convertMM2M()

◆ createConvexHull2D()

◆ deg2rad()

◆ distPointLine()

◆ distPointSegment()

◆ eigen3f2rpy()

◆ eigen4f2axisangle()

◆ eigen4f2posrpy()

◆ eigen4f2quat() [1/2]

◆ eigen4f2quat() [2/2]

◆ eigen4f2rpy() [1/3]

◆ eigen4f2rpy() [2/3]

◆ eigen4f2rpy() [3/3]

◆ ensureOrthonormalBasis()

◆ findNormal()

◆ fmod()

◆ getAngle() [1/2]

◆ getAngle() [2/2]

◆ getBasisTransformation() [1/2]

◆ getBasisTransformation() [2/2]

◆ getCartesianPoseDiff()

◆ getConjugated()

◆ getConvexHullCenter()

◆ getDampedLeastSquareInverse()

◆ getDamping() [1/2]

◆ getDamping() [2/2]

◆ getDelta() [1/2]

◆ getDelta() [2/2]

◆ getDistancePointPlane()

◆ getDot()

◆ getFloorPlane()

◆ getInverse()

◆ getMean()

◆ getPermutation()

◆ getPoseDiff()

◆ getPseudoInverse()

◆ getPseudoInverseD()

◆ getPseudoInverseDamped()

◆ getPseudoInverseDampedD()

◆ getRotation()

◆ getTransformXMLString() [1/4]

◆ getTransformXMLString() [2/4]

◆ getTransformXMLString() [3/4]

◆ getTransformXMLString() [4/4]

◆ getTranslation() [1/2]

◆ getTranslation() [2/2]

◆ getTriangleArea()

◆ GramSchmidt()

◆ hopf2quat()

◆ ILerp()

◆ intersectOOBBPlane()

◆ intersectPlanes()

◆ intersectSegmentPlane()

◆ isInside()

◆ isLeft()

◆ isValid()