Geometry.hpp

/*
 * Copyright (c) 2011-2021, 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_