DART  6.10.1
Geometry.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_MATH_GEOMETRY_HPP_
34 #define DART_MATH_GEOMETRY_HPP_
35 
36 #include <Eigen/Dense>
37 
39 #include "dart/math/Constants.hpp"
40 #include "dart/math/MathTypes.hpp"
41 
42 namespace dart {
43 namespace math {
44 
46 Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d& _v);
47 
49 Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m);
50 
51 //------------------------------------------------------------------------------
53 Eigen::Quaterniond expToQuat(const Eigen::Vector3d& _v);
54 
56 Eigen::Vector3d quatToExp(const Eigen::Quaterniond& _q);
57 
59 Eigen::Vector3d rotatePoint(
60  const Eigen::Quaterniond& _q, const Eigen::Vector3d& _pt);
61 
63 Eigen::Vector3d rotatePoint(
64  const Eigen::Quaterniond& _q, double _x, double _y, double _z);
65 
67 Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond& _q, int _el);
68 
70 Eigen::Matrix3d quatSecondDeriv(
71  const Eigen::Quaterniond& _q, int _el1, int _el2);
72 
73 //------------------------------------------------------------------------------
76 Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d& _angle);
77 
80 Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d& _angle);
81 
84 Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d& _angle);
85 
88 Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d& _angle);
89 
92 Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d& _angle);
93 
96 Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d& _angle);
97 
100 Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d& _angle);
101 
104 Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d& _angle);
105 
108 Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d& _angle);
109 
113 Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d& _angle);
114 
117 Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d& _angle);
118 
122 Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle);
123 
124 //------------------------------------------------------------------------------
126 Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d& _R);
127 
129 Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d& _R);
130 
132 // Eigen::Vector3d matrixToEulerXZX(const Eigen::Matrix3d& R);
133 
135 Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d& _R);
136 
138 // Eigen::Vector3d matrixToEulerYXY(const Eigen::Matrix3d& R);
139 
141 Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d& _R);
142 
144 Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d& _R);
145 
147 // Eigen::Vector3d matrixToEulerYZY(const Eigen::Matrix3d& R);
148 
150 Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d& _R);
151 
153 Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d& _R);
154 
156 // Eigen::Vector3d matrixToEulerZXZ(const Eigen::Matrix3d& R);
157 
159 // Eigen::Vector3d matrixToEulerZYZ(const Eigen::Matrix3d& R);
160 
161 //------------------------------------------------------------------------------
163 Eigen::Isometry3d expMap(const Eigen::Vector6d& _S);
164 
169 Eigen::Isometry3d expAngular(const Eigen::Vector3d& _s);
170 
172 Eigen::Matrix3d expMapRot(const Eigen::Vector3d& _expmap);
173 
175 Eigen::Matrix3d expMapJac(const Eigen::Vector3d& _expmap);
176 
178 Eigen::Matrix3d expMapJacDot(
179  const Eigen::Vector3d& _expmap, const Eigen::Vector3d& _qdot);
180 
183 Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d& _expmap, int _qi);
184 
188 Eigen::Vector3d logMap(const Eigen::Matrix3d& _R);
189 
191 Eigen::Vector6d logMap(const Eigen::Isometry3d& _T);
192 
193 //------------------------------------------------------------------------------
200 // SE3 Normalize(const SE3& T);
201 
203 // Axis Reparameterize(const Axis& s);
204 
205 //------------------------------------------------------------------------------
209 Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
210 
212 Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T);
213 
215 template <typename Derived>
216 typename Derived::PlainObject AdTJac(
217  const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
218 {
219  // Check the number of rows is 6 at compile time
220  EIGEN_STATIC_ASSERT(
221  Derived::RowsAtCompileTime == 6,
222  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
223 
224  typename Derived::PlainObject ret(_J.rows(), _J.cols());
225 
226  // Compute AdT column by column
227  for (int i = 0; i < _J.cols(); ++i)
228  ret.col(i) = AdT(_T, _J.col(i));
229 
230  return ret;
231 }
232 
234 template <typename Derived>
235 typename Derived::PlainObject AdTJacFixed(
236  const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
237 {
238  // Check if _J is fixed size Jacobian
239  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
240 
241  // Check the number of rows is 6 at compile time
242  EIGEN_STATIC_ASSERT(
243  Derived::RowsAtCompileTime == 6,
244  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
245 
246  typename Derived::PlainObject ret(_J.rows(), _J.cols());
247 
248  // Compute AdT
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>();
253 
254  return ret;
255 }
256 
258 Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
259 
262  const Eigen::Isometry3d& _T, const Eigen::Vector3d& _w);
263 
266  const Eigen::Isometry3d& _T, const Eigen::Vector3d& _v);
267 
269 // se3 AdP(const Vec3& p, const se3& s);
270 
272 template <typename Derived>
273 typename Derived::PlainObject AdRJac(
274  const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
275 {
276  EIGEN_STATIC_ASSERT(
277  Derived::RowsAtCompileTime == 6,
278  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
279 
280  typename Derived::PlainObject ret(_J.rows(), _J.cols());
281 
282  ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
283 
284  ret.template bottomRows<3>().noalias()
285  = _T.linear() * _J.template bottomRows<3>();
286 
287  return ret;
288 }
289 
290 template <typename Derived>
291 typename Derived::PlainObject AdRInvJac(
292  const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
293 {
294  EIGEN_STATIC_ASSERT(
295  Derived::RowsAtCompileTime == 6,
296  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
297 
298  typename Derived::PlainObject ret(_J.rows(), _J.cols());
299 
300  ret.template topRows<3>().noalias()
301  = _T.linear().transpose() * _J.template topRows<3>();
302 
303  ret.template bottomRows<3>().noalias()
304  = _T.linear().transpose() * _J.template bottomRows<3>();
305 
306  return ret;
307 }
308 
309 template <typename Derived>
310 typename Derived::PlainObject adJac(
311  const Eigen::Vector6d& _V, const Eigen::MatrixBase<Derived>& _J)
312 {
313  EIGEN_STATIC_ASSERT(
314  Derived::RowsAtCompileTime == 6,
315  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
316 
317  typename Derived::PlainObject ret(_J.rows(), _J.cols());
318 
319  ret.template topRows<3>().noalias()
320  = -_J.template topRows<3>().colwise().cross(_V.head<3>());
321 
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>());
325 
326  return ret;
327 }
328 
330 Eigen::Vector6d AdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
331 
333 template <typename Derived>
334 typename Derived::PlainObject AdInvTJac(
335  const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
336 {
337  // Check the number of rows is 6 at compile time
338  EIGEN_STATIC_ASSERT(
339  Derived::RowsAtCompileTime == 6,
340  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
341 
342  typename Derived::PlainObject ret(_J.rows(), _J.cols());
343 
344  // Compute AdInvT column by column
345  for (int i = 0; i < _J.cols(); ++i)
346  ret.col(i) = AdInvT(_T, _J.col(i));
347 
348  return ret;
349 }
350 
352 template <typename Derived>
353 typename Derived::PlainObject AdInvTJacFixed(
354  const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
355 {
356  // Check if _J is fixed size Jacobian
357  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
358 
359  // Check the number of rows is 6 at compile time
360  EIGEN_STATIC_ASSERT(
361  Derived::RowsAtCompileTime == 6,
362  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
363 
364  typename Derived::PlainObject ret(_J.rows(), _J.cols());
365 
366  // Compute AdInvT
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()));
373 
374  return ret;
375 }
376 
378 // Eigen::Vector3d AdInvTLinear(const Eigen::Isometry3d& T,
379 // const Eigen::Vector3d& v);
380 
382 // Axis AdInvTAngular(const SE3& T, const Axis& w);
383 
385 // se3 AdInvR(const SE3& T, const se3& V);
386 
389  const Eigen::Isometry3d& _T, const Eigen::Vector3d& _v);
390 
394 Eigen::Vector6d dAdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
395 
397 // dse3 dAdTLinear(const SE3& T, const Vec3& F);
398 
400 Eigen::Vector6d dAdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
401 
403 Eigen::Vector6d dAdInvR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
404 
406 // dse3 dAdInvPLinear(const Vec3& p, const Vec3& F);
407 
411 Eigen::Vector6d ad(const Eigen::Vector6d& _X, const Eigen::Vector6d& _Y);
412 
414 // Vec3 ad_Vec3_se3(const Vec3& v, const se3& S);
415 
417 // Axis ad_Axis_Axis(const Axis& w, const Axis& v);
418 
422 Eigen::Vector6d dad(const Eigen::Vector6d& _s, const Eigen::Vector6d& _t);
423 
425 Inertia transformInertia(const Eigen::Isometry3d& _T, const Inertia& _AI);
426 
429 Eigen::Matrix3d parallelAxisTheorem(
430  const Eigen::Matrix3d& _original,
431  const Eigen::Vector3d& _comShift,
432  double _mass);
433 
435 {
436  AXIS_X = 0,
437  AXIS_Y = 1,
438  AXIS_Z = 2
439 };
440 
444 Eigen::Matrix3d computeRotation(
445  const Eigen::Vector3d& axis, AxisType axisType = AxisType::AXIS_X);
446 
450 Eigen::Isometry3d computeTransform(
451  const Eigen::Vector3d& axis,
452  const Eigen::Vector3d& translation,
453  AxisType axisType = AxisType::AXIS_X);
454 
456 DART_DEPRECATED(6.0)
457 Eigen::Isometry3d getFrameOriginAxisZ(
458  const Eigen::Vector3d& _origin, const Eigen::Vector3d& _axisZ);
459 
462 bool verifyRotation(const Eigen::Matrix3d& _R);
463 
466 bool verifyTransform(const Eigen::Isometry3d& _T);
467 
470 inline double wrapToPi(double angle)
471 {
472  constexpr auto pi = constantsd::pi();
473 
474  return std::fmod(angle + pi, 2 * pi) - pi;
475 }
476 
477 template <typename MatrixType, typename ReturnType>
478 void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
479 {
480  int rank = 0;
481  // TODO(MXG): Replace this with _SVD.rank() once the latest Eigen is released
482  if (_SVD.nonzeroSingularValues() > 0)
483  {
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)
489  --i;
490  rank = i + 1;
491  }
492 
493  int cols = _SVD.matrixV().cols(), rows = _SVD.matrixV().rows();
494  _NS = _SVD.matrixV().block(0, rank, rows, cols - rank);
495 }
496 
497 template <typename MatrixType, typename ReturnType>
498 void computeNullSpace(const MatrixType& _M, ReturnType& _NS)
499 {
500  Eigen::JacobiSVD<MatrixType> svd(_M, Eigen::ComputeFullV);
501  extractNullSpace(svd, _NS);
502 }
503 
504 typedef std::vector<Eigen::Vector3d> SupportGeometry;
505 
507 
513  const SupportGeometry& _geometry,
514  const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
515  const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
516 
522  std::vector<std::size_t>& _originalIndices,
523  const SupportGeometry& _geometry,
524  const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
525  const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
526 
529 
533  std::vector<std::size_t>& _originalIndices, const SupportPolygon& _points);
534 
536 Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull);
537 
541 {
542 
547 
548 };
549 
553  Eigen::Vector2d& _intersectionPoint,
554  const Eigen::Vector2d& a1,
555  const Eigen::Vector2d& a2,
556  const Eigen::Vector2d& b1,
557  const Eigen::Vector2d& b2);
558 
560 double cross(const Eigen::Vector2d& _v1, const Eigen::Vector2d& _v2);
561 
564  const Eigen::Vector2d& _p,
565  const SupportPolygon& _support,
566  bool _includeEdge = true);
567 
570 Eigen::Vector2d computeClosestPointOnLineSegment(
571  const Eigen::Vector2d& _p,
572  const Eigen::Vector2d& _s1,
573  const Eigen::Vector2d& _s2);
574 
578  const Eigen::Vector2d& _p, const SupportPolygon& _support);
579 
583  std::size_t& _index1,
584  std::size_t& _index2,
585  const Eigen::Vector2d& _p,
586  const SupportPolygon& _support);
587 
588 // Represents a bounding box with minimum and maximum coordinates.
590 {
591 public:
592  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
593 
594  BoundingBox();
595  BoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max);
596 
597  inline const Eigen::Vector3d& getMin() const
598  {
599  return mMin;
600  }
601  inline const Eigen::Vector3d& getMax() const
602  {
603  return mMax;
604  }
605 
606  inline void setMin(const Eigen::Vector3d& min)
607  {
608  mMin = min;
609  }
610  inline void setMax(const Eigen::Vector3d& max)
611  {
612  mMax = max;
613  }
614 
615  // \brief Centroid of the bounding box (i.e average of min and max)
616  inline Eigen::Vector3d computeCenter() const
617  {
618  return (mMax + mMin) * 0.5;
619  }
620  // \brief Coordinates of the maximum corner with respect to the centroid.
621  inline Eigen::Vector3d computeHalfExtents() const
622  {
623  return (mMax - mMin) * 0.5;
624  }
625  // \brief Length of each of the sides of the bounding box.
626  inline Eigen::Vector3d computeFullExtents() const
627  {
628  return (mMax - mMin);
629  }
630 
631 protected:
632  // \brief minimum coordinates of the bounding box
633  Eigen::Vector3d mMin;
634  // \brief maximum coordinates of the bounding box
635  Eigen::Vector3d mMax;
636 };
637 
638 } // namespace math
639 } // namespace dart
640 
641 #endif // DART_MATH_GEOMETRY_HPP_
#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