33 #ifndef DART_MATH_GEOMETRY_HPP_
34 #define DART_MATH_GEOMETRY_HPP_
36 #include <Eigen/Dense>
53 Eigen::Quaterniond
expToQuat(
const Eigen::Vector3d& _v);
56 Eigen::Vector3d
quatToExp(
const Eigen::Quaterniond& _q);
60 const Eigen::Quaterniond& _q,
const Eigen::Vector3d& _pt);
64 const Eigen::Quaterniond& _q,
double _x,
double _y,
double _z);
67 Eigen::Matrix3d
quatDeriv(
const Eigen::Quaterniond& _q,
int _el);
71 const Eigen::Quaterniond& _q,
int _el1,
int _el2);
169 Eigen::Isometry3d
expAngular(
const Eigen::Vector3d& _s);
172 Eigen::Matrix3d
expMapRot(
const Eigen::Vector3d& _expmap);
175 Eigen::Matrix3d
expMapJac(
const Eigen::Vector3d& _expmap);
179 const Eigen::Vector3d& _expmap,
const Eigen::Vector3d& _qdot);
183 Eigen::Matrix3d
expMapJacDeriv(
const Eigen::Vector3d& _expmap,
int _qi);
188 Eigen::Vector3d
logMap(
const Eigen::Matrix3d& _R);
215 template <
typename Derived>
217 const Eigen::Isometry3d& _T,
const Eigen::MatrixBase<Derived>& _J)
221 Derived::RowsAtCompileTime == 6,
222 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
224 typename Derived::PlainObject ret(_J.rows(), _J.cols());
227 for (
int i = 0; i < _J.cols(); ++i)
228 ret.col(i) =
AdT(_T, _J.col(i));
234 template <
typename Derived>
236 const Eigen::Isometry3d& _T,
const Eigen::MatrixBase<Derived>& _J)
239 EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
243 Derived::RowsAtCompileTime == 6,
244 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
246 typename Derived::PlainObject ret(_J.rows(), _J.cols());
249 ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
250 ret.template bottomRows<3>().noalias()
251 = -ret.template topRows<3>().colwise().cross(_T.translation())
252 + _T.linear() * _J.template bottomRows<3>();
262 const Eigen::Isometry3d& _T,
const Eigen::Vector3d& _w);
266 const Eigen::Isometry3d& _T,
const Eigen::Vector3d& _v);
272 template <
typename Derived>
274 const Eigen::Isometry3d& _T,
const Eigen::MatrixBase<Derived>& _J)
277 Derived::RowsAtCompileTime == 6,
278 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
280 typename Derived::PlainObject ret(_J.rows(), _J.cols());
282 ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
284 ret.template bottomRows<3>().noalias()
285 = _T.linear() * _J.template bottomRows<3>();
290 template <
typename Derived>
292 const Eigen::Isometry3d& _T,
const Eigen::MatrixBase<Derived>& _J)
295 Derived::RowsAtCompileTime == 6,
296 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
298 typename Derived::PlainObject ret(_J.rows(), _J.cols());
300 ret.template topRows<3>().noalias()
301 = _T.linear().transpose() * _J.template topRows<3>();
303 ret.template bottomRows<3>().noalias()
304 = _T.linear().transpose() * _J.template bottomRows<3>();
309 template <
typename Derived>
310 typename Derived::PlainObject
adJac(
314 Derived::RowsAtCompileTime == 6,
315 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
317 typename Derived::PlainObject ret(_J.rows(), _J.cols());
319 ret.template topRows<3>().noalias()
320 = -_J.template topRows<3>().colwise().cross(_V.head<3>());
322 ret.template bottomRows<3>().noalias()
323 = -_J.template bottomRows<3>().colwise().cross(_V.head<3>())
324 - _J.template topRows<3>().colwise().cross(_V.tail<3>());
333 template <
typename Derived>
335 const Eigen::Isometry3d& _T,
const Eigen::MatrixBase<Derived>& _J)
339 Derived::RowsAtCompileTime == 6,
340 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
342 typename Derived::PlainObject ret(_J.rows(), _J.cols());
345 for (
int i = 0; i < _J.cols(); ++i)
346 ret.col(i) =
AdInvT(_T, _J.col(i));
352 template <
typename Derived>
354 const Eigen::Isometry3d& _T,
const Eigen::MatrixBase<Derived>& _J)
357 EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
361 Derived::RowsAtCompileTime == 6,
362 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
364 typename Derived::PlainObject ret(_J.rows(), _J.cols());
367 ret.template topRows<3>().noalias()
368 = _T.linear().transpose() * _J.template topRows<3>();
369 ret.template bottomRows<3>().noalias()
370 = _T.linear().transpose()
371 * (_J.template bottomRows<3>()
372 + _J.template topRows<3>().colwise().cross(_T.translation()));
389 const Eigen::Isometry3d& _T,
const Eigen::Vector3d& _v);
430 const Eigen::Matrix3d& _original,
431 const Eigen::Vector3d& _comShift,
451 const Eigen::Vector3d& axis,
452 const Eigen::Vector3d& translation,
458 const
Eigen::Vector3d& _origin, const
Eigen::Vector3d& _axisZ);
474 return std::fmod(angle + pi, 2 * pi) - pi;
477 template <
typename MatrixType,
typename ReturnType>
482 if (_SVD.nonzeroSingularValues() > 0)
484 double thresh = std::max(
485 _SVD.singularValues().coeff(0) * 1e-10,
486 std::numeric_limits<double>::min());
487 int i = _SVD.nonzeroSingularValues() - 1;
488 while (i >= 0 && _SVD.singularValues().coeff(i) < thresh)
493 int cols = _SVD.matrixV().cols(), rows = _SVD.matrixV().rows();
494 _NS = _SVD.matrixV().block(0, rank, rows, cols - rank);
497 template <
typename MatrixType,
typename ReturnType>
500 Eigen::JacobiSVD<MatrixType> svd(_M, Eigen::ComputeFullV);
514 const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
515 const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
522 std::vector<std::size_t>& _originalIndices,
524 const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
525 const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
533 std::vector<std::size_t>& _originalIndices,
const SupportPolygon& _points);
553 Eigen::Vector2d& _intersectionPoint,
554 const Eigen::Vector2d& a1,
555 const Eigen::Vector2d& a2,
556 const Eigen::Vector2d& b1,
557 const Eigen::Vector2d& b2);
560 double cross(
const Eigen::Vector2d& _v1,
const Eigen::Vector2d& _v2);
564 const Eigen::Vector2d& _p,
566 bool _includeEdge =
true);
571 const Eigen::Vector2d& _p,
572 const Eigen::Vector2d& _s1,
573 const Eigen::Vector2d& _s2);
583 std::size_t& _index1,
584 std::size_t& _index2,
585 const Eigen::Vector2d& _p,
592 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
595 BoundingBox(
const Eigen::Vector3d& min,
const Eigen::Vector3d& max);
597 inline const Eigen::Vector3d&
getMin()
const
601 inline const Eigen::Vector3d&
getMax()
const
606 inline void setMin(
const Eigen::Vector3d& min)
610 inline void setMax(
const Eigen::Vector3d& max)
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
Definition: Geometry.hpp:590
const Eigen::Vector3d & getMax() const
Definition: Geometry.hpp:601
void setMin(const Eigen::Vector3d &min)
Definition: Geometry.hpp:606
Eigen::Vector3d mMax
Definition: Geometry.hpp:635
Eigen::Vector3d computeHalfExtents() const
Definition: Geometry.hpp:621
Eigen::Vector3d computeCenter() const
Definition: Geometry.hpp:616
Eigen::Vector3d computeFullExtents() const
Definition: Geometry.hpp:626
Eigen::Vector3d mMin
Definition: Geometry.hpp:633
void setMax(const Eigen::Vector3d &max)
Definition: Geometry.hpp:610
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoundingBox()
Definition: Geometry.cpp:2098
const Eigen::Vector3d & getMin() const
Definition: Geometry.hpp:597
Definition: Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: Memory.hpp:71
Eigen::Vector6d dad(const Eigen::Vector6d &_s, const Eigen::Vector6d &_t)
fast version of ad(se3(Eigen_Vec3(0), v), S)
Definition: Geometry.cpp:1405
IntersectionResult 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 ...
Definition: Geometry.cpp:1877
Derived::PlainObject AdInvTJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for dynamic size Jacobian.
Definition: Geometry.hpp:334
Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d &_origin, const Eigen::Vector3d &_axisZ)
Generate frame given origin and z-axis.
Definition: Geometry.cpp:1601
Derived::PlainObject AdInvTJacFixed(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for fixed size Jacobian.
Definition: Geometry.hpp:353
Eigen::Vector3d quatToExp(const Eigen::Quaterniond &_q)
Definition: Geometry.cpp:66
Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d &T)
Get linear transformation matrix of Adjoint mapping.
Definition: Geometry.cpp:696
Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d &_angle)
Given EulerXYZ angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotY(ang...
Definition: Geometry.cpp:915
Derived::PlainObject AdRJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Change coordinate Frame of a Jacobian.
Definition: Geometry.hpp:273
Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d &_angle)
Given Euler XYX angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotY(an...
Definition: Geometry.cpp:883
bool verifyRotation(const Eigen::Matrix3d &_T)
Check if determinant of _R is equat to 1 and all the elements are not NaN values.
Definition: Geometry.cpp:1512
AxisType
Definition: Geometry.hpp:435
@ AXIS_Y
Definition: Geometry.hpp:437
@ AXIS_X
Definition: Geometry.hpp:436
@ AXIS_Z
Definition: Geometry.hpp:438
Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d &_angle)
Given EulerZXZ angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotX(ang...
Definition: Geometry.cpp:1199
SupportPolygon computeSupportPolgyon(const SupportGeometry &_geometry, const Eigen::Vector3d &_axis1, const Eigen::Vector3d &_axis2)
Project the support geometry points onto a plane with the given axes and then compute their convex hu...
Definition: Geometry.cpp:1608
Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d &_R)
get the Euler XYX angle from R
Definition: Geometry.cpp:75
Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d &_R)
get the Euler ZXY angle from R
Definition: Geometry.cpp:235
Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d &_angle)
Given EulerZYZ angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotY(ang...
Definition: Geometry.cpp:1231
Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d &_q, int _qi)
computes the derivative of the Jacobian of the expmap wrt to _qi indexed dof; _qi {0,...
Definition: Geometry.cpp:538
Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d &_angle)
Given EulerZXY angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotX(ang...
Definition: Geometry.cpp:1135
Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d &_angle)
Given EulerYXY angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotX(ang...
Definition: Geometry.cpp:1011
Eigen::Matrix3d expMapJac(const Eigen::Vector3d &_q)
Computes the Jacobian of the expmap.
Definition: Geometry.cpp:488
Eigen::Matrix3d computeRotation(const Eigen::Vector3d &axis, const AxisType axisType)
Compute a rotation matrix from a vector.
Definition: Geometry.cpp:1554
double wrapToPi(double angle)
Compute the angle (in the range of -pi to +pi) which ignores any full rotations.
Definition: Geometry.hpp:470
Eigen::Matrix3d expMapJacDot(const Eigen::Vector3d &_q, const Eigen::Vector3d &_qdot)
Computes the time derivative of the expmap Jacobian.
Definition: Geometry.cpp:505
Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d &_R)
get the Euler XYZ angle from R
Definition: Geometry.cpp:117
Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d &_m)
Definition: Geometry.cpp:1523
Eigen::Vector2d 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...
Definition: Geometry.cpp:1999
Eigen::Vector2d computeCentroidOfHull(const SupportPolygon &_convexHull)
Compute the centroid of a polygon, assuming the polygon is a convex hull.
Definition: Geometry.cpp:1665
Eigen::Vector6d AdTLinear(const Eigen::Isometry3d &_T, const Eigen::Vector3d &_v)
fast version of Ad(T, se3(0, v))
Definition: Geometry.cpp:734
Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d &_angle)
Given EulerZYX angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotY(ang...
Definition: Geometry.cpp:1167
bool isInsideSupportPolygon(const Eigen::Vector2d &_p, const SupportPolygon &_support, bool _includeEdge)
Returns true if the point _p is inside the support polygon.
Definition: Geometry.cpp:1933
Eigen::Vector6d AdInvRLinear(const Eigen::Isometry3d &_T, const Eigen::Vector3d &_v)
Fast version of Ad(Inv([R 0; 0 1]), se3(0, v))
Definition: Geometry.cpp:800
Eigen::Vector3d rotatePoint(const Eigen::Quaterniond &_q, const Eigen::Vector3d &_pt)
Definition: Geometry.cpp:437
Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d &_angle)
Given EulerXZX angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:947
Eigen::Isometry3d computeTransform(const Eigen::Vector3d &axis, const Eigen::Vector3d &translation, AxisType axisType)
Compute a transform from a vector and a position.
Definition: Geometry.cpp:1584
Eigen::Matrix3d 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 bee...
Definition: Geometry.cpp:1498
Eigen::Isometry3d expAngular(const Eigen::Vector3d &_s)
fast version of Exp(se3(s, 0))
Definition: Geometry.cpp:1313
bool 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 valu...
Definition: Geometry.cpp:1517
Derived::PlainObject AdRInvJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Definition: Geometry.hpp:291
Eigen::Vector6d AdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
fast version of Ad(Inv(T), V)
Definition: Geometry.cpp:776
Eigen::Vector3d logMap(const Eigen::Matrix3d &_R)
Log mapping.
Definition: Geometry.cpp:565
Eigen::Isometry3d expMap(const Eigen::Vector6d &_S)
Exponential mapping.
Definition: Geometry.cpp:1266
Eigen::Vector6d AdR(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
Fast version of Ad([R 0; 0 1], V)
Definition: Geometry.cpp:709
double cross(const Eigen::Vector2d &_v1, const Eigen::Vector2d &_v2)
Compute a 2D cross product.
Definition: Geometry.cpp:1927
Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d &_angle)
Given EulerXZY angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:979
Eigen::Vector6d dAdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
fast version of dAd(Inv(T), F)
Definition: Geometry.cpp:850
Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d &_angle)
Given EulerYZX angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:1073
SupportPolygon computeConvexHull(const SupportPolygon &_points)
Computes the convex hull of a set of 2D points.
Definition: Geometry.cpp:1657
IntersectionResult
Intersection_t is returned by the computeIntersection() function to indicate whether there was a vali...
Definition: Geometry.hpp:541
@ INTERSECTING
An intersection was found.
Definition: Geometry.hpp:543
@ BEYOND_ENDPOINTS
There is no intersection because the end points do not expand far enough.
Definition: Geometry.hpp:545
@ PARALLEL
The line segments are parallel.
Definition: Geometry.hpp:544
Derived::PlainObject AdTJacFixed(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for fixed size Jacobian.
Definition: Geometry.hpp:235
void extractNullSpace(const Eigen::JacobiSVD< MatrixType > &_SVD, ReturnType &_NS)
Definition: Geometry.hpp:478
Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d &_R)
get the Euler YXZ angle from R
Definition: Geometry.cpp:263
Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d &_R)
get the Euler XZY angle from R
Definition: Geometry.cpp:179
Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond &_q, int _el)
Definition: Geometry.cpp:292
Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d &_v)
Definition: Geometry.cpp:1539
void computeNullSpace(const MatrixType &_M, ReturnType &_NS)
Definition: Geometry.hpp:498
Eigen::Matrix3d quatSecondDeriv(const Eigen::Quaterniond &, int _el1, int _el2)
Definition: Geometry.cpp:349
Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d &_R)
get the Euler YZX angle from R
Definition: Geometry.cpp:207
Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d &_angle)
Given EulerYXZ angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotX(ang...
Definition: Geometry.cpp:1043
Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d &_R)
get the Euler ZYX angle from R
Definition: Geometry.cpp:151
Derived::PlainObject adJac(const Eigen::Vector6d &_V, const Eigen::MatrixBase< Derived > &_J)
Definition: Geometry.hpp:310
Eigen::Vector6d AdTAngular(const Eigen::Isometry3d &_T, const Eigen::Vector3d &_w)
fast version of Ad(T, se3(w, 0))
Definition: Geometry.cpp:721
Derived::PlainObject AdTJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for dynamic size Jacobian.
Definition: Geometry.hpp:216
Eigen::Vector6d dAdT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
dual adjoint mapping
Definition: Geometry.cpp:824
Eigen::Vector6d ad(const Eigen::Vector6d &_X, const Eigen::Vector6d &_Y)
adjoint mapping
Definition: Geometry.cpp:808
Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d &_angle)
Given EulerYZY angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:1103
Eigen::Vector2d 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.
Definition: Geometry.cpp:2041
std::vector< Eigen::Vector3d > SupportGeometry
Definition: Geometry.hpp:504
Eigen::Vector6d AdT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
Rectify the rotation part so as that it satifies the orthogonality condition.
Definition: Geometry.cpp:682
Inertia transformInertia(const Eigen::Isometry3d &_T, const Inertia &_I)
Definition: Geometry.cpp:1414
Eigen::Matrix3d expMapRot(const Eigen::Vector3d &_q)
Computes the Rotation matrix from a given expmap vector.
Definition: Geometry.cpp:471
Eigen::Vector6d dAdInvR(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
fast version of dAd(Inv([R 0; 0 1]), F)
Definition: Geometry.cpp:859
Eigen::Quaterniond expToQuat(const Eigen::Vector3d &_v)
Definition: Geometry.cpp:50
Eigen::Matrix6d Inertia
Definition: MathTypes.hpp:111
common::aligned_vector< Eigen::Vector2d > SupportPolygon
Definition: Geometry.hpp:506
Definition: BulletCollisionDetector.cpp:65
static constexpr T pi()
Definition: Constants.hpp:45