Geometry.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef DART_MATH_GEOMETRY_HPP_
#define DART_MATH_GEOMETRY_HPP_
 
#include <Eigen/Dense>
 
#include "dart/common/Deprecated.hpp"
#include "dart/math/Constants.hpp"
#include "dart/math/MathTypes.hpp"
 
namespace dart {
namespace math {
 
Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d& _v);
 
Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m);
 
//------------------------------------------------------------------------------
Eigen::Quaterniond expToQuat(const Eigen::Vector3d& _v);
 
Eigen::Vector3d quatToExp(const Eigen::Quaterniond& _q);
 
Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
                            const Eigen::Vector3d& _pt);
 
Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
                            double _x, double _y, double _z);
 
Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond& _q, int _el);
 
Eigen::Matrix3d quatSecondDeriv(const Eigen::Quaterniond& _q,
                                int _el1, int _el2);
 
//------------------------------------------------------------------------------
Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d& _angle);
 
Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle);
 
//------------------------------------------------------------------------------
Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d& _R);
 
Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d& _R);
 
// Eigen::Vector3d matrixToEulerXZX(const Eigen::Matrix3d& R);
 
Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d& _R);
 
// Eigen::Vector3d matrixToEulerYXY(const Eigen::Matrix3d& R);
 
Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d& _R);
 
Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d& _R);
 
// Eigen::Vector3d matrixToEulerYZY(const Eigen::Matrix3d& R);
 
Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d& _R);
 
Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d& _R);
 
// Eigen::Vector3d matrixToEulerZXZ(const Eigen::Matrix3d& R);
 
// Eigen::Vector3d matrixToEulerZYZ(const Eigen::Matrix3d& R);
 
//------------------------------------------------------------------------------
Eigen::Isometry3d expMap(const Eigen::Vector6d& _S);
 
Eigen::Isometry3d expAngular(const Eigen::Vector3d& _s);
 
Eigen::Matrix3d expMapRot(const Eigen::Vector3d& _expmap);
 
Eigen::Matrix3d expMapJac(const Eigen::Vector3d& _expmap);
 
Eigen::Matrix3d expMapJacDot(const Eigen::Vector3d& _expmap,
                             const Eigen::Vector3d& _qdot);
 
Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d& _expmap, int _qi);
 
Eigen::Vector3d logMap(const Eigen::Matrix3d& _R);
 
Eigen::Vector6d logMap(const Eigen::Isometry3d& _T);
 
//------------------------------------------------------------------------------
// SE3 Normalize(const SE3& T);
 
// Axis Reparameterize(const Axis& s);
 
//------------------------------------------------------------------------------
Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
 
Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T);
 
template<typename Derived>
typename Derived::PlainObject AdTJac(const Eigen::Isometry3d& _T,
                                     const Eigen::MatrixBase<Derived>& _J)
{
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
 
  typename Derived::PlainObject ret(_J.rows(), _J.cols());
 
  // Compute AdT column by column
  for (int i = 0; i < _J.cols(); ++i)
    ret.col(i) = AdT(_T, _J.col(i));
 
  return ret;
}
 
template<typename Derived>
typename Derived::PlainObject AdTJacFixed(const Eigen::Isometry3d& _T,
                                          const Eigen::MatrixBase<Derived>& _J)
{
  // Check if _J is fixed size Jacobian
  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
 
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
 
  typename Derived::PlainObject ret(_J.rows(), _J.cols());
 
  // Compute AdT
  ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
  ret.template bottomRows<3>().noalias()
      = -ret.template topRows<3>().colwise().cross(_T.translation())
        + _T.linear() * _J.template bottomRows<3>();
 
  return ret;
}
 
Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
 
Eigen::Vector6d AdTAngular(const Eigen::Isometry3d& _T,
                           const Eigen::Vector3d& _w);
 
Eigen::Vector6d AdTLinear(const Eigen::Isometry3d& _T,
                          const Eigen::Vector3d& _v);
 
// se3 AdP(const Vec3& p, const se3& s);
 
template<typename Derived>
typename Derived::PlainObject AdRJac(const Eigen::Isometry3d& _T,
                                     const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
 
  typename Derived::PlainObject ret(_J.rows(), _J.cols());
 
  ret.template topRows<3>().noalias() =
      _T.linear() * _J.template topRows<3>();
 
  ret.template bottomRows<3>().noalias() =
      _T.linear() * _J.template bottomRows<3>();
 
  return ret;
}
 
template<typename Derived>
typename Derived::PlainObject AdRInvJac(const Eigen::Isometry3d& _T,
                                        const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
 
  typename Derived::PlainObject ret(_J.rows(), _J.cols());
 
  ret.template topRows<3>().noalias() =
      _T.linear().transpose() * _J.template topRows<3>();
 
  ret.template bottomRows<3>().noalias() =
      _T.linear().transpose() * _J.template bottomRows<3>();
 
  return ret;
}
 
template<typename Derived>
typename Derived::PlainObject adJac(const Eigen::Vector6d& _V,
                                    const Eigen::MatrixBase<Derived>& _J)
{
  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
 
  typename Derived::PlainObject ret(_J.rows(), _J.cols());
 
  ret.template topRows<3>().noalias() =
      - _J.template topRows<3>().colwise().cross(_V.head<3>());
 
  ret.template bottomRows<3>().noalias() =
      - _J.template bottomRows<3>().colwise().cross(_V.head<3>())
      - _J.template topRows<3>().colwise().cross(_V.tail<3>());
 
  return ret;
}
 
Eigen::Vector6d AdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
 
template<typename Derived>
typename Derived::PlainObject AdInvTJac(const Eigen::Isometry3d& _T,
                                        const Eigen::MatrixBase<Derived>& _J)
{
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
 
  typename Derived::PlainObject ret(_J.rows(), _J.cols());
 
  // Compute AdInvT column by column
  for (int i = 0; i < _J.cols(); ++i)
    ret.col(i) = AdInvT(_T, _J.col(i));
 
  return ret;
}
 
template<typename Derived>
typename Derived::PlainObject AdInvTJacFixed(
    const Eigen::Isometry3d& _T,
    const Eigen::MatrixBase<Derived>& _J)
{
  // Check if _J is fixed size Jacobian
  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
 
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
                      THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
 
  typename Derived::PlainObject ret(_J.rows(), _J.cols());
 
  // Compute AdInvT
  ret.template topRows<3>().noalias()
      = _T.linear().transpose() * _J.template topRows<3>();
  ret.template bottomRows<3>().noalias()
      = _T.linear().transpose()
        * (_J.template bottomRows<3>()
           + _J.template topRows<3>().colwise().cross(_T.translation()));
 
  return ret;
}
 
// Eigen::Vector3d AdInvTLinear(const Eigen::Isometry3d& T,
//                             const Eigen::Vector3d& v);
 
// Axis AdInvTAngular(const SE3& T, const Axis& w);
 
// se3 AdInvR(const SE3& T, const se3& V);
 
Eigen::Vector6d AdInvRLinear(const Eigen::Isometry3d& _T,
                             const Eigen::Vector3d& _v);
 
Eigen::Vector6d dAdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
 
// dse3 dAdTLinear(const SE3& T, const Vec3& F);
 
Eigen::Vector6d dAdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
 
Eigen::Vector6d dAdInvR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
 
// dse3 dAdInvPLinear(const Vec3& p, const Vec3& F);
 
Eigen::Vector6d ad(const Eigen::Vector6d& _X, const Eigen::Vector6d& _Y);
 
// Vec3 ad_Vec3_se3(const Vec3& v, const se3& S);
 
// Axis ad_Axis_Axis(const Axis& w, const Axis& v);
 
Eigen::Vector6d dad(const Eigen::Vector6d& _s, const Eigen::Vector6d& _t);
 
Inertia transformInertia(const Eigen::Isometry3d& _T, const Inertia& _AI);
 
Eigen::Matrix3d parallelAxisTheorem(const Eigen::Matrix3d& _original,
                                    const Eigen::Vector3d& _comShift,
                                    double _mass);
 
enum AxisType
{
  AXIS_X = 0,
  AXIS_Y = 1,
  AXIS_Z = 2
};
 
Eigen::Matrix3d computeRotation(const Eigen::Vector3d& axis,
                                AxisType axisType = AxisType::AXIS_X);
 
Eigen::Isometry3d computeTransform(const Eigen::Vector3d& axis,
                                   const Eigen::Vector3d& translation,
                                   AxisType axisType = AxisType::AXIS_X);
 
DART_DEPRECATED(6.0)
Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin,
                                      const Eigen::Vector3d& _axisZ);
 
bool verifyRotation(const Eigen::Matrix3d& _R);
 
bool verifyTransform(const Eigen::Isometry3d& _T);
 
inline double wrapToPi(double angle)
{
  constexpr auto pi = constantsd::pi();
 
  return std::fmod(angle+pi, 2*pi) - pi;
}
 
template <typename MatrixType, typename ReturnType>
void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
{
  int rank = 0;
  // TODO(MXG): Replace this with _SVD.rank() once the latest Eigen is released
  if(_SVD.nonzeroSingularValues() > 0)
  {
    double thresh = std::max(_SVD.singularValues().coeff(0)*1e-10,
                             std::numeric_limits<double>::min());
    int i = _SVD.nonzeroSingularValues()-1;
    while( i >= 0 && _SVD.singularValues().coeff(i) < thresh )
      --i;
    rank = i+1;
  }
 
  int cols = _SVD.matrixV().cols(), rows = _SVD.matrixV().rows();
  _NS = _SVD.matrixV().block(0, rank, rows, cols-rank);
}
 
template <typename MatrixType, typename ReturnType>
void computeNullSpace(const MatrixType& _M, ReturnType& _NS)
{
  Eigen::JacobiSVD<MatrixType> svd(_M, Eigen::ComputeFullV);
  extractNullSpace(svd, _NS);
}
 
typedef std::vector<Eigen::Vector3d> SupportGeometry;
 
typedef common::aligned_vector<Eigen::Vector2d> SupportPolygon;
 
SupportPolygon computeSupportPolgyon(
    const SupportGeometry& _geometry,
    const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
    const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
 
SupportPolygon computeSupportPolgyon(
    std::vector<std::size_t>& _originalIndices,
    const SupportGeometry& _geometry,
    const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
    const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
 
SupportPolygon computeConvexHull(const SupportPolygon& _points);
 
SupportPolygon computeConvexHull(std::vector<std::size_t>& _originalIndices,
                                 const SupportPolygon& _points);
 
Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull);
 
enum IntersectionResult {
 
  INTERSECTING = 0,   
  PARALLEL,           
  BEYOND_ENDPOINTS    
 
};
 
IntersectionResult computeIntersection(
    Eigen::Vector2d& _intersectionPoint,
    const Eigen::Vector2d& a1,
    const Eigen::Vector2d& a2,
    const Eigen::Vector2d& b1,
    const Eigen::Vector2d& b2);
 
double cross(const Eigen::Vector2d& _v1, const Eigen::Vector2d& _v2);
 
bool isInsideSupportPolygon(const Eigen::Vector2d& _p,
                            const SupportPolygon& _support,
                            bool _includeEdge = true);
 
Eigen::Vector2d computeClosestPointOnLineSegment(const Eigen::Vector2d& _p,
                                                 const Eigen::Vector2d& _s1,
                                                 const Eigen::Vector2d& _s2);
 
Eigen::Vector2d computeClosestPointOnSupportPolygon(
    const Eigen::Vector2d& _p,
    const SupportPolygon& _support);
 
Eigen::Vector2d computeClosestPointOnSupportPolygon(
    std::size_t& _index1,
    std::size_t& _index2,
    const Eigen::Vector2d& _p,
    const SupportPolygon& _support);
 
 
// Represents a bounding box with minimum and maximum coordinates.
class BoundingBox {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        BoundingBox();
        BoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max);
 
        inline const Eigen::Vector3d& getMin() const {  return mMin; }
        inline const Eigen::Vector3d& getMax() const { return mMax; }
 
        inline void setMin(const Eigen::Vector3d& min) { mMin = min; }
        inline void setMax(const Eigen::Vector3d& max) { mMax = max; }
 
        // \brief Centroid of the bounding box (i.e average of min and max)
        inline Eigen::Vector3d computeCenter() const { return (mMax + mMin) * 0.5; }
        // \brief Coordinates of the maximum corner with respect to the centroid.
        inline Eigen::Vector3d computeHalfExtents() const { return (mMax - mMin) * 0.5; }
        // \brief Length of each of the sides of the bounding box.
        inline Eigen::Vector3d computeFullExtents() const { return (mMax - mMin); }
 
    protected:
        // \brief minimum coordinates of the bounding box
        Eigen::Vector3d mMin;
        // \brief maximum coordinates of the bounding box
        Eigen::Vector3d mMax;
};
 
}  // namespace math
}  // namespace dart
 
#endif  // DART_MATH_GEOMETRY_HPP_