DART  6.6.2
Geometry.cpp File Reference
#include "dart/math/Geometry.hpp"
#include <algorithm>
#include <cassert>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>
#include "dart/common/Console.hpp"
#include "dart/math/Helpers.hpp"

Classes

struct  dart::math::HullAngle
 

Namespaces

 dart
 
 dart::math
 

Macros

#define DART_EPSILON   1e-6
 
#define EPSILON_EXPMAP_THETA   1.0e-3
 

Functions

Eigen::Quaterniond dart::math::expToQuat (const Eigen::Vector3d &_v)
 
Eigen::Vector3d dart::math::quatToExp (const Eigen::Quaterniond &_q)
 
Eigen::Vector3d dart::math::matrixToEulerXYX (const Eigen::Matrix3d &_R)
 get the Euler XYX angle from R More...
 
Eigen::Vector3d dart::math::matrixToEulerXYZ (const Eigen::Matrix3d &_R)
 get the Euler XYZ angle from R More...
 
Eigen::Vector3d dart::math::matrixToEulerZYX (const Eigen::Matrix3d &_R)
 get the Euler ZYX angle from R More...
 
Eigen::Vector3d dart::math::matrixToEulerXZY (const Eigen::Matrix3d &_R)
 get the Euler XZY angle from R More...
 
Eigen::Vector3d dart::math::matrixToEulerYZX (const Eigen::Matrix3d &_R)
 get the Euler YZX angle from R More...
 
Eigen::Vector3d dart::math::matrixToEulerZXY (const Eigen::Matrix3d &_R)
 get the Euler ZXY angle from R More...
 
Eigen::Vector3d dart::math::matrixToEulerYXZ (const Eigen::Matrix3d &_R)
 get the Euler YXZ angle from R More...
 
Eigen::Matrix3d dart::math::quatDeriv (const Eigen::Quaterniond &_q, int _el)
 
Eigen::Matrix3d dart::math::quatSecondDeriv (const Eigen::Quaterniond &_q, int _el1, int _el2)
 
Eigen::Vector3d dart::math::rotatePoint (const Eigen::Quaterniond &_q, const Eigen::Vector3d &_pt)
 
Eigen::Vector3d dart::math::rotatePoint (const Eigen::Quaterniond &_q, double _x, double _y, double _z)
 
Eigen::Matrix3d dart::math::expMapRot (const Eigen::Vector3d &_expmap)
 Computes the Rotation matrix from a given expmap vector. More...
 
Eigen::Matrix3d dart::math::expMapJac (const Eigen::Vector3d &_expmap)
 Computes the Jacobian of the expmap. More...
 
Eigen::Matrix3d dart::math::expMapJacDot (const Eigen::Vector3d &_expmap, const Eigen::Vector3d &_qdot)
 Computes the time derivative of the expmap Jacobian. More...
 
Eigen::Matrix3d dart::math::expMapJacDeriv (const Eigen::Vector3d &_expmap, int _qi)
 computes the derivative of the Jacobian of the expmap wrt to _qi indexed dof; _qi \in {0,1,2} More...
 
Eigen::Vector3d dart::math::logMap (const Eigen::Matrix3d &_R)
 Log mapping. More...
 
Eigen::Vector6d dart::math::logMap (const Eigen::Isometry3d &_T)
 Log mapping. More...
 
Eigen::Vector6d dart::math::AdT (const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
 Rectify the rotation part so as that it satifies the orthogonality condition. More...
 
Eigen::Matrix6d dart::math::getAdTMatrix (const Eigen::Isometry3d &T)
 Get linear transformation matrix of Adjoint mapping. More...
 
Eigen::Vector6d dart::math::AdR (const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
 Fast version of Ad([R 0; 0 1], V) More...
 
Eigen::Vector6d dart::math::AdTAngular (const Eigen::Isometry3d &_T, const Eigen::Vector3d &_w)
 fast version of Ad(T, se3(w, 0)) More...
 
Eigen::Vector6d dart::math::AdTLinear (const Eigen::Isometry3d &_T, const Eigen::Vector3d &_v)
 fast version of Ad(T, se3(0, v)) More...
 
Eigen::Vector6d dart::math::AdInvT (const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
 fast version of Ad(Inv(T), V) More...
 
Eigen::Vector6d dart::math::AdInvRLinear (const Eigen::Isometry3d &_T, const Eigen::Vector3d &_v)
 Fast version of Ad(Inv([R 0; 0 1]), se3(0, v)) More...
 
Eigen::Vector6d dart::math::ad (const Eigen::Vector6d &_X, const Eigen::Vector6d &_Y)
 adjoint mapping More...
 
Eigen::Vector6d dart::math::dAdT (const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
 dual adjoint mapping More...
 
Eigen::Vector6d dart::math::dAdInvT (const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
 fast version of dAd(Inv(T), F) More...
 
Eigen::Vector6d dart::math::dAdInvR (const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
 fast version of dAd(Inv([R 0; 0 1]), F) More...
 
Eigen::Matrix3d dart::math::eulerXYXToMatrix (const Eigen::Vector3d &_angle)
 Given Euler XYX angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotY(angle(1)) * RotX(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerXYZToMatrix (const Eigen::Vector3d &_angle)
 Given EulerXYZ angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotY(angle(1)) * RotZ(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerXZXToMatrix (const Eigen::Vector3d &_angle)
 Given EulerXZX angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotZ(angle(1)) * RotX(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerXZYToMatrix (const Eigen::Vector3d &_angle)
 Given EulerXZY angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotZ(angle(1)) * RotY(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerYXYToMatrix (const Eigen::Vector3d &_angle)
 Given EulerYXY angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotX(angle(1)) * RotY(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerYXZToMatrix (const Eigen::Vector3d &_angle)
 Given EulerYXZ angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotX(angle(1)) * RotZ(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerYZXToMatrix (const Eigen::Vector3d &_angle)
 Given EulerYZX angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotZ(angle(1)) * RotX(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerYZYToMatrix (const Eigen::Vector3d &_angle)
 Given EulerYZY angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotZ(angle(1)) * RotY(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerZXYToMatrix (const Eigen::Vector3d &_angle)
 Given EulerZXY angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotX(angle(1)) * RotY(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerZYXToMatrix (const Eigen::Vector3d &_angle)
 Given EulerZYX angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotY(angle(1)) * RotX(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerZXZToMatrix (const Eigen::Vector3d &_angle)
 Given EulerZXZ angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotX(angle(1)) * RotZ(angle(2)). More...
 
Eigen::Matrix3d dart::math::eulerZYZToMatrix (const Eigen::Vector3d &_angle)
 Given EulerZYZ angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotY(angle(1)) * RotZ(angle(2)). More...
 
Eigen::Isometry3d dart::math::expMap (const Eigen::Vector6d &_S)
 Exponential mapping. More...
 
Eigen::Isometry3d dart::math::expAngular (const Eigen::Vector3d &_s)
 fast version of Exp(se3(s, 0)) More...
 
Eigen::Vector6d dart::math::dad (const Eigen::Vector6d &_s, const Eigen::Vector6d &_t)
 fast version of ad(se3(Eigen_Vec3(0), v), S) More...
 
Inertia dart::math::transformInertia (const Eigen::Isometry3d &_T, const Inertia &_AI)
 
Eigen::Matrix3d dart::math::parallelAxisTheorem (const Eigen::Matrix3d &_original, const Eigen::Vector3d &_comShift, double _mass)
 Use the Parallel Axis Theorem to compute the moment of inertia of a body whose center of mass has been shifted from the origin. More...
 
bool dart::math::verifyRotation (const Eigen::Matrix3d &_R)
 Check if determinant of _R is equat to 1 and all the elements are not NaN values. More...
 
bool dart::math::verifyTransform (const Eigen::Isometry3d &_T)
 Check if determinant of the rotational part of _T is equat to 1 and all the elements are not NaN values. More...
 
Eigen::Vector3d dart::math::fromSkewSymmetric (const Eigen::Matrix3d &_m)
 
Eigen::Matrix3d dart::math::makeSkewSymmetric (const Eigen::Vector3d &_v)
 
Eigen::Matrix3d dart::math::computeRotation (const Eigen::Vector3d &axis, AxisType axisType=AxisType::AXIS_X)
 Compute a rotation matrix from a vector. More...
 
Eigen::Isometry3d dart::math::computeTransform (const Eigen::Vector3d &axis, const Eigen::Vector3d &translation, AxisType axisType=AxisType::AXIS_X)
 Compute a transform from a vector and a position. More...
 
Eigen::Isometry3d dart::math::getFrameOriginAxisZ (const Eigen::Vector3d &_origin, const Eigen::Vector3d &_axisZ)
 Generate frame given origin and z-axis. More...
 
SupportPolygon dart::math::computeSupportPolgyon (const SupportGeometry &_geometry, const Eigen::Vector3d &_axis1=Eigen::Vector3d::UnitX(), const Eigen::Vector3d &_axis2=Eigen::Vector3d::UnitY())
 Project the support geometry points onto a plane with the given axes and then compute their convex hull, which will take the form of a polgyon. More...
 
SupportPolygon dart::math::computeSupportPolgyon (std::vector< std::size_t > &_originalIndices, const SupportGeometry &_geometry, const Eigen::Vector3d &_axis1=Eigen::Vector3d::UnitX(), const Eigen::Vector3d &_axis2=Eigen::Vector3d::UnitY())
 Same as computeSupportPolgyon, except you can pass in a std::vector<std::size_t> which will have the same size as the returned SupportPolygon, and each entry will contain the original index of each point in the SupportPolygon. More...
 
static bool dart::math::HullAngleComparison (const HullAngle &a, const HullAngle &b)
 
SupportPolygon dart::math::computeConvexHull (const SupportPolygon &_points)
 Computes the convex hull of a set of 2D points. More...
 
Eigen::Vector2d dart::math::computeCentroidOfHull (const SupportPolygon &_convexHull)
 Compute the centroid of a polygon, assuming the polygon is a convex hull. More...
 
static bool dart::math::isLeftTurn (const Eigen::Vector2d &p1, const Eigen::Vector2d &p2, const Eigen::Vector2d &p3)
 
SupportPolygon dart::math::computeConvexHull (std::vector< std::size_t > &_originalIndices, const SupportPolygon &_points)
 Computes the convex hull of a set of 2D points and fills in _originalIndices with the original index of each entry in the returned SupportPolygon. More...
 
IntersectionResult dart::math::computeIntersection (Eigen::Vector2d &_intersectionPoint, const Eigen::Vector2d &a1, const Eigen::Vector2d &a2, const Eigen::Vector2d &b1, const Eigen::Vector2d &b2)
 Compute the intersection between a line segment that goes from a1 -> a2 and a line segment that goes from b1 -> b2. More...
 
double dart::math::cross (const Eigen::Vector2d &_v1, const Eigen::Vector2d &_v2)
 Compute a 2D cross product. More...
 
bool dart::math::isInsideSupportPolygon (const Eigen::Vector2d &_p, const SupportPolygon &_support, bool _includeEdge=true)
 Returns true if the point _p is inside the support polygon. More...
 
Eigen::Vector2d dart::math::computeClosestPointOnLineSegment (const Eigen::Vector2d &_p, const Eigen::Vector2d &_s1, const Eigen::Vector2d &_s2)
 Returns the point which is closest to _p that also lays on the line segment that goes from _s1 -> _s2. More...
 
Eigen::Vector2d dart::math::computeClosestPointOnSupportPolygon (const Eigen::Vector2d &_p, const SupportPolygon &_support)
 Returns the point which is closest to _p that also lays on the edge of the support polygon. More...
 
Eigen::Vector2d dart::math::computeClosestPointOnSupportPolygon (std::size_t &_index1, std::size_t &_index2, const Eigen::Vector2d &_p, const SupportPolygon &_support)
 Same as closestPointOnSupportPolygon, but also fills in _index1 and _index2 with the indices of the line segment. More...
 

Macro Definition Documentation

◆ DART_EPSILON

#define DART_EPSILON   1e-6

◆ EPSILON_EXPMAP_THETA

#define EPSILON_EXPMAP_THETA   1.0e-3