Simox  2.3.50
VirtualRobot::MathTools Namespace Reference

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 boost::shared_ptr< ConvexHull2DConvexHull2DPtr
 
typedef boost::shared_ptr< ConvexHull3DConvexHull3DPtr
 
typedef boost::shared_ptr< ConvexHull6DConvexHull6DPtr
 

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)
 
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)
 
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)
 
Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT posrpy2eigen4f (const Eigen::Vector3f &pos, const Eigen::Vector3f &rpy)
 
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::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 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)
 
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

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

Enumeration Type Documentation

Enumerator
eParallel 
eNoIntersection 
eIntersection 

Function Documentation

Eigen::Matrix3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2eigen3f ( const Eigen::Vector3f &  axis,
float  angle 
)
Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2eigen4f ( const Eigen::Vector3f &  axis,
float  angle 
)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2eigen4f ( const Eigen::Vector3f &  axis,
float  angle,
Eigen::Matrix4f &  storeResult 
)
MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::axisangle2quat ( const Eigen::Vector3f &  axis,
float  angle 
)
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.

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)

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::convertMM2M ( const std::vector< ContactPoint points,
std::vector< ContactPoint > &  storeResult 
)
MathTools::ConvexHull2DPtr VirtualRobot::MathTools::createConvexHull2D ( const std::vector< Eigen::Vector2f > &  points)
float VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::deg2rad ( float  deg)
template<typename VectorT >
float VirtualRobot::MathTools::distPointLine ( const BaseLine< VectorT > &  l,
const VectorT &  p 
)
inline

Returns the distance of vector p to line l.

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.

void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2axisangle ( const Eigen::Matrix4f &  m,
Eigen::Vector3f &  storeAxis,
float &  storeAngle 
)
MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2quat ( const Eigen::Matrix4f &  m)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2quat ( const Eigen::Matrix4f &  m,
Quaternion q 
)
void VirtualRobot::MathTools::eigen4f2rpy ( const Eigen::Matrix4f &  m,
float  x[6] 
)

Convert homogeneous matrix to translation and rpy rotation.

Parameters
mThe matrix to be converted
xThe result is stored in this float array (x,y,z,roll,pitch,yaw)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2rpy ( const Eigen::Matrix4f &  m,
Eigen::Vector3f &  storeRPY 
)
Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::eigen4f2rpy ( const Eigen::Matrix4f &  m)
bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::ensureOrthonormalBasis ( Eigen::Vector3f &  x,
Eigen::Vector3f &  y,
Eigen::Vector3f &  z 
)
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.

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

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

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

Returns angle between v1 and v2 [rad].

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
basisSrcThe initial basis vectors.
basisDstThe final basis vectors.
Returns
The transformation matrix T.
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
basisSrcThe column vectors are the initial basis vectors
basisDstThe column vectors are the final basis vectors.
Returns
The transformation matrix T.
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

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

Return the conjugated quaternion.

Eigen::Vector2f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getConvexHullCenter ( ConvexHull2DPtr  ch)
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.

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
m1The first pose.
m2The second pose.
storeDetalPosThe position delta is stored here.
storeDeltaRotThe orientation delta is stored here [radian]
float VirtualRobot::MathTools::getDistancePointPlane ( const Eigen::Vector3f &  point,
const Plane plane 
)
float VirtualRobot::MathTools::getDot ( const Quaternion q1,
const Quaternion q2 
)

returns q1 dot q2

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

Create a floor plane.

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

Return the inverse quaternion.

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

Computes mean orientation of quaternions.

Eigen::VectorXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPermutation ( const Eigen::VectorXf &  inputA,
const Eigen::VectorXf &  inputB,
unsigned int  i 
)
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
p1First Pose.
p2Second Pose.
storePosDiffThe translational absolute distance between p1 and p2 is stored here
storeRotDiffRadThe rotational absolute distance between p1 and p2 is stored here (radian)
Returns
Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverse ( const Eigen::MatrixXf &  m,
float  tol = 1e-5f 
)

Returns the Pseudo inverse matrix.

Eigen::MatrixXd VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverseD ( const Eigen::MatrixXd &  m,
double  tol = 1e-5 
)
Eigen::MatrixXf VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverseDamped ( const Eigen::MatrixXf &  m,
float  lambda = 1.0f 
)

Returns the damped Pseudo inverse matrix.

Eigen::MatrixXd VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getPseudoInverseDampedD ( const Eigen::MatrixXd &  m,
double  lambda = 1.0 
)
MathTools::Quaternion VirtualRobot::MathTools::getRotation ( const Eigen::Vector3f &  from,
const Eigen::Vector3f &  to 
)

Return rotation that converts vector from to vector to.

std::string VirtualRobot::MathTools::getTransformXMLString ( const Eigen::Matrix4f &  m,
int  tabs,
bool  skipMatrixTag = false 
)
std::string VirtualRobot::MathTools::getTransformXMLString ( const Eigen::Matrix4f &  m,
const std::string &  tabs,
bool  skipMatrixTag = false 
)
std::string VirtualRobot::MathTools::getTransformXMLString ( const Eigen::Matrix3f &  m,
int  tabs,
bool  skipMatrixTag = false 
)
std::string VirtualRobot::MathTools::getTransformXMLString ( const Eigen::Matrix3f &  m,
const std::string &  tabs,
bool  skipMatrixTag = false 
)
Eigen::Vector3f VirtualRobot::MathTools::getTranslation ( const Eigen::Matrix4f &  m)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::getTranslation ( const Eigen::Matrix4f &  m,
Eigen::Vector3f &  v 
)
bool VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::GramSchmidt ( std::vector< Eigen::VectorXf > &  basis)

perform the Gram-Schmidt method for building an orthonormal basis

MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::hopf2quat ( const Eigen::Vector3f &  hopf)
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
oobbThe oobb
planeThe plane
storeResultIn case an intersection exists, the intersection area is defined by these points
Returns
If the oobb does not intersect eNoIntersection is returned, otherwise eIntersection.
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!

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
segmentThe segment
planeThe plane
storeResultIn 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.
bool VirtualRobot::MathTools::isInside ( const Eigen::Vector2f &  p,
ConvexHull2DPtr  hull 
)
float VirtualRobot::MathTools::isLeft ( Eigen::Vector2f  P0,
Eigen::Vector2f  P1,
Eigen::Vector2f  P2 
)
inline
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)

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

Returns q1*q2.

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

Returns nearest point to p on line l.

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.

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.

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

Get the corresponding point in 3D.

void VirtualRobot::MathTools::posrpy2eigen4f ( const float  x[6],
Eigen::Matrix4f &  m 
)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::posrpy2eigen4f ( const Eigen::Vector3f &  pos,
const Eigen::Vector3f &  rpy,
Eigen::Matrix4f &  m 
)
Eigen::Matrix4f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::posrpy2eigen4f ( const Eigen::Vector3f &  pos,
const Eigen::Vector3f &  rpy 
)
int VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::pow_int ( int  a,
int  b 
)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print ( const ContactPoint p)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print ( const std::vector< ContactPoint > &  points)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print ( const Eigen::VectorXf &  v,
bool  endline = true 
)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::print ( const std::vector< float > &  v,
bool  endline = true 
)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::printMat ( const Eigen::MatrixXf &  m,
bool  endline = true 
)
Eigen::Vector3f VirtualRobot::MathTools::projectPointToPlane ( const Eigen::Vector3f &  point,
const Plane plane 
)

Get the projected point in 3D.

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

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

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.
Eigen::Matrix4f VirtualRobot::MathTools::quat2eigen4f ( const Quaternion  q)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::quat2eigen4f ( float  x,
float  y,
float  z,
float  w,
Eigen::Matrix4f &  m 
)
void VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::quat2eigen4f ( const Quaternion  q,
Eigen::Matrix4f &  m 
)
Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::quat2hopf ( const Quaternion q)
float VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::rad2deg ( float  rad)
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.

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.

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.

Eigen::Matrix4f VirtualRobot::MathTools::rpy2eigen4f ( float  r,
float  p,
float  y 
)
MathTools::Quaternion VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::slerp ( const MathTools::Quaternion q1,
const MathTools::Quaternion q2,
float  alpha 
)

Apply the slerp interpolation

Parameters
q1The first quaternion
q2The second quaternion
alphaA value between 0 and 1
Returns
The intermediate quaternion
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".

Eigen::Vector3f VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::toPosition ( const SphericalCoord sc)
VirtualRobot::MathTools::SphericalCoord VIRTUAL_ROBOT_IMPORT_EXPORT VirtualRobot::MathTools::toSphericalCoords ( const Eigen::Vector3f &  pos)
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

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