/*
 * 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.
 */

#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"

#define DART_EPSILON 1e-6

namespace dart {
namespace math {

Eigen::Quaterniond expToQuat(const Eigen::Vector3d& _v) {
  double mag = _v.norm();

  if (mag > 1e-10) {
    Eigen::Quaterniond q(Eigen::AngleAxisd(mag, _v / mag));
    return q;
  } else {
    Eigen::Quaterniond q(1, 0, 0, 0);
    return q;
  }
}

Eigen::Vector3d quatToExp(const Eigen::Quaterniond& _q) {
  Eigen::AngleAxisd aa(_q);
  Eigen::Vector3d v = aa.axis();
  return v*aa.angle();
}

// Reference:
// http://www.geometrictools.com/LibMathematics/Algebra/Wm5Matrix3.inl
Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d& _R) {
  // +-           -+   +-                                                -+
  // | r00 r01 r02 |   |  cy      sy*sx1               sy*cx1             |
  // | r10 r11 r12 | = |  sy*sx0  cx0*cx1-cy*sx0*sx1  -cy*cx1*sx0-cx0*sx1 |
  // | r20 r21 r22 |   | -sy*cx0  cx1*sx0+cy*cx0*sx1   cy*cx0*cx1-sx0*sx1 |
  // +-           -+   +-                                                -+

  if (_R(0, 0) < 1.0) {
    if (_R(0, 0) > -1.0) {
      // y_angle  = acos(r00)
      // x0_angle = atan2(r10,-r20)
      // x1_angle = atan2(r01,r02)
      double y  = acos(_R(0, 0));
      double x0 = atan2(_R(1, 0), -_R(2, 0));
      double x1 = atan2(_R(0, 1), _R(0, 2));
      // return EA_UNIQUE;
      return Eigen::Vector3d(x0, y, x1);
    } else {
      // Not a unique solution:  x1_angle - x0_angle = atan2(-r12,r11)
      double y  = constantsd::pi();
      double x0 = -atan2(-_R(1, 2), _R(1, 1));
      double x1 = 0.0;
      // return EA_NOT_UNIQUE_DIF;
      return Eigen::Vector3d(x0, y, x1);
    }
  } else {
    // Not a unique solution:  x1_angle + x0_angle = atan2(-r12,r11)
    double y  = 0.0;
    double x0 = -atan2(-_R(1, 2), _R(1, 1));
    double x1 = 0.0;
    // return EA_NOT_UNIQUE_SUM;
    return Eigen::Vector3d(x0, y, x1);
  }
}

Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d& _R) {
  // +-           -+   +-                                        -+
  // | r00 r01 r02 |   |  cy*cz           -cy*sz            sy    |
  // | r10 r11 r12 | = |  cz*sx*sy+cx*sz   cx*cz-sx*sy*sz  -cy*sx |
  // | r20 r21 r22 |   | -cx*cz*sy+sx*sz   cz*sx+cx*sy*sz   cx*cy |
  // +-           -+   +-                                        -+

  double x, y, z;

  if (_R(0, 2) > (1.0-DART_EPSILON)) {
    z = atan2(_R(1, 0), _R(1, 1));
    y = constantsd::half_pi();
    x = 0.0;
    return Eigen::Vector3d(x, y, z);
  }

  if (_R(0, 2) < -(1.0-DART_EPSILON)) {
    z = atan2(_R(1, 0), _R(1, 1));
    y = -constantsd::half_pi();
    x = 0.0;
    return Eigen::Vector3d(x, y, z);
  }

  z = -atan2(_R(0, 1), _R(0, 0));
  y = asin(_R(0, 2));
  x = -atan2(_R(1, 2), _R(2, 2));

  // order of return is the order of input
  return Eigen::Vector3d(x, y, z);
}

Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d& _R) {
  double x, y, z;

  if (_R(2, 0) > (1.0-DART_EPSILON)) {
    x = atan2(_R(0, 1), _R(0, 2));
    y = -constantsd::half_pi();
    z = 0.0;
    return Eigen::Vector3d(z, y, x);
  }

  if (_R(2, 0) < -(1.0-DART_EPSILON)) {
    x = atan2(_R(0, 1), _R(0, 2));
    y = constantsd::half_pi();
    z = 0.0;
    return Eigen::Vector3d(z, y, x);
  }

  x = atan2(_R(2, 1), _R(2, 2));
  y = -asin(_R(2, 0));
  z = atan2(_R(1, 0), _R(0, 0));

  // order of return is the order of input
  return Eigen::Vector3d(z, y, x);
}

Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d& _R) {
  double x, y, z;

  if (_R(0, 1) > (1.0-DART_EPSILON)) {
    y = atan2(_R(1, 2), _R(1, 0));
    z = -constantsd::half_pi();
    x = 0.0;
    return Eigen::Vector3d(x, z, y);
  }

  if (_R(0, 1) < -(1.0-DART_EPSILON)) {
    y = atan2(_R(1, 2), _R(1, 0));
    z = constantsd::half_pi();
    x = 0.0;
    return Eigen::Vector3d(x, z, y);
  }

  y = atan2(_R(0, 2), _R(0, 0));
  z = -asin(_R(0, 1));
  x = atan2(_R(2, 1), _R(1, 1));

  // order of return is the order of input
  return Eigen::Vector3d(x, z, y);
}

Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d& _R) {
  double x, y, z;

  if (_R(1, 0) > (1.0 - DART_EPSILON)) {
    x = -atan2(_R(0, 2), _R(0, 1));
    z = constantsd::half_pi();
    y = 0.0;
    return Eigen::Vector3d(y, z, x);
  }

  if (_R(1, 0) < -(1.0 - DART_EPSILON)) {
    x = -atan2(_R(0, 2), _R(0, 1));
    z = -constantsd::half_pi();
    y = 0.0;
    return Eigen::Vector3d(y, z, x);
  }

  x = -atan2(_R(1, 2), _R(1, 1));
  z = asin(_R(1, 0));
  y = -atan2(_R(2, 0), _R(0, 0));

  // order of return is the order of input
  return Eigen::Vector3d(y, z, x);
}

Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d& _R) {
  double x, y, z;

  if (_R(2, 1) > (1.0-DART_EPSILON)) {
    y = atan2(_R(0, 2), _R(0, 0));
    x = constantsd::half_pi();
    z = 0.0;
    return Eigen::Vector3d(z, x, y);
  }

  if (_R(2, 1) < -(1.0-DART_EPSILON)) {
    y = atan2(_R(0, 2), _R(0, 0));
    x = -constantsd::half_pi();
    z = 0.0;
    return Eigen::Vector3d(z, x, y);
  }

  y = -atan2(_R(2, 0), _R(2, 2));
  x = asin(_R(2, 1));
  z = -atan2(_R(0, 1), _R(1, 1));

  // order of return is the order of input
  return Eigen::Vector3d(z, x, y);
}

Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d& _R) {
  double x, y, z;

  if (_R(1, 2) > (1.0-DART_EPSILON)) {
    z = -atan2(_R(0, 1), _R(0, 0));
    x = -constantsd::half_pi();
    y = 0.0;
    return Eigen::Vector3d(y, x, z);
  }

  if (_R(1, 2) < -(1.0-DART_EPSILON)) {
    z = -atan2(_R(0, 1), _R(0, 0));
    x = constantsd::half_pi();
    y = 0.0;
    return Eigen::Vector3d(y, x, z);
  }

  z = atan2(_R(1, 0), _R(1, 1));
  x = -asin(_R(1, 2));
  y = atan2(_R(0, 2), _R(2, 2));

  // order of return is the order of input
  return Eigen::Vector3d(y, x, z);
}

// get the derivative of rotation matrix wrt el no.
Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond& _q, int _el) {
  Eigen::Matrix3d mat = Eigen::Matrix3d::Zero();

  switch (_el) {
    case 0:  // wrt w
      mat(0, 0) = _q.w();
      mat(1, 1) = _q.w();
      mat(2, 2) = _q.w();
      mat(0, 1) = -_q.z();
      mat(1, 0) = _q.z();
      mat(0, 2) = _q.y();
      mat(2, 0) = -_q.y();
      mat(1, 2) = -_q.x();
      mat(2, 1) = _q.x();
      break;
    case 1:  // wrt x
      mat(0, 0) = _q.x();
      mat(1, 1) = -_q.x();
      mat(2, 2) = -_q.x();
      mat(0, 1) = _q.y();
      mat(1, 0) = _q.y();
      mat(0, 2) = _q.z();
      mat(2, 0) = _q.z();
      mat(1, 2) = -_q.w();
      mat(2, 1) = _q.w();
      break;
    case 2:  // wrt y
      mat(0, 0) = -_q.y();
      mat(1, 1) = _q.y();
      mat(2, 2) = -_q.y();
      mat(0, 1) = _q.x();
      mat(1, 0) = _q.x();
      mat(0, 2) = _q.w();
      mat(2, 0) = -_q.w();
      mat(1, 2) = _q.z();
      mat(2, 1) = _q.z();
      break;
    case 3:  // wrt z
      mat(0, 0) = -_q.z();
      mat(1, 1) = -_q.z();
      mat(2, 2) = _q.z();
      mat(0, 1) = -_q.w();
      mat(1, 0) = _q.w();
      mat(0, 2) = _q.x();
      mat(2, 0) = _q.x();
      mat(1, 2) = _q.y();
      mat(2, 1) = _q.y();
      break;
    default:
      break;
  }

  return 2*mat;
}

Eigen::Matrix3d quatSecondDeriv(const Eigen::Quaterniond& /*_q*/,
                                int _el1, int _el2) {
  Eigen::Matrix3d mat = Eigen::Matrix3d::Zero();

  if (_el1 == _el2) {  // wrt same dof
    switch (_el1) {
      case 0:  // wrt w
        mat(0, 0) = 1;
        mat(1, 1) = 1;
        mat(2, 2) = 1;
        break;
      case 1:  // wrt x
        mat(0, 0) = 1;
        mat(1, 1) = -1;
        mat(2, 2) = -1;
        break;
      case 2:  // wrt y
        mat(0, 0) = -1;
        mat(1, 1) = 1;
        mat(2, 2) =-1;
        break;
      case 3:  // wrt z
        mat(0, 0) = -1;
        mat(1, 1) = -1;
        mat(2, 2) = 1;
        break;
    }
  } else {  // wrt different dofs
    // arrange in increasing order
    if (_el1 > _el2) {
      int temp = _el2;
      _el2 = _el1;
      _el1 = temp;
    }

    switch (_el1) {
      case 0:  // wrt w
        switch (_el2) {
          case 1:  // wrt x
            mat(1, 2) = -1;
            mat(2, 1) = 1;
            break;
          case 2:  // wrt y
            mat(0, 2) = 1;
            mat(2, 0) = -1;
            break;
          case 3:  // wrt z
            mat(0, 1) = -1;
            mat(1, 0) = 1;
            break;
        }
        break;
      case 1:  // wrt x
        switch (_el2) {
          case 2:  // wrt y
            mat(0, 1) = 1;
            mat(1, 0) = 1;
            break;
          case 3:  // wrt z
            mat(0, 2) = 1;
            mat(2, 0) = 1;
            break;
        }
        break;
      case 2:  // wrt y
        switch (_el2) {
          case 3:  // wrt z
            mat(1, 2) = 1;
            mat(2, 1) = 1;
            break;
        }
        break;
    }
  }

  return 2*mat;
}

Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
                            const Eigen::Vector3d& _pt) {
  Eigen::Quaterniond quat_pt(0, _pt[0], _pt[1], _pt[2]);
  Eigen::Quaterniond qinv = _q.inverse();

  Eigen::Quaterniond rot = _q*quat_pt*qinv;

  // check below - assuming same format of point achieved
  Eigen::Vector3d temp;
//  VLOG(1) << "Point before: " << 0 << " "
//          << pt.x << " " << pt.y << " " << pt.z << "\n";
//  VLOG(1) << "Point after:  "
//          << rot.x << " " << rot.y << " " << rot.z << " " << rot.w << "\n";
  temp[0] = rot.x();
  temp[1] = rot.y();
  temp[2] = rot.z();

  //  VLOG(1) << "Point after rotation: "
  //          << temp[0] << " " << temp[1] << " " << temp[2] << endl;
  return temp;
}

Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
                            double _x, double _y, double _z) {
  Eigen::Vector3d pt(_x, _y, _z);
  return rotatePoint(_q, pt);
}

// ----------- expmap computations -------------

#define EPSILON_EXPMAP_THETA 1.0e-3

Eigen::Matrix3d expMapRot(const Eigen::Vector3d& _q) {
  double theta = _q.norm();

  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d qss =  math::makeSkewSymmetric(_q);
  Eigen::Matrix3d qss2 =  qss*qss;

  if (theta < EPSILON_EXPMAP_THETA)
    R = Eigen::Matrix3d::Identity() + qss + 0.5*qss2;
  else
    R = Eigen::Matrix3d::Identity()
        + (sin(theta)/theta)*qss
        + ((1-cos(theta))/(theta*theta))*qss2;

  return R;
}

Eigen::Matrix3d expMapJac(const Eigen::Vector3d& _q) {
  double theta = _q.norm();

  Eigen::Matrix3d J = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d qss =  math::makeSkewSymmetric(_q);
  Eigen::Matrix3d qss2 =  qss*qss;

  if (theta < EPSILON_EXPMAP_THETA)
    J = Eigen::Matrix3d::Identity() + 0.5*qss +  (1.0/6.0)*qss2;
  else
    J = Eigen::Matrix3d::Identity()
        + ((1-cos(theta))/(theta*theta))*qss
        + ((theta-sin(theta))/(theta*theta*theta))*qss2;

  return J;
}

Eigen::Matrix3d expMapJacDot(const Eigen::Vector3d& _q,
                             const Eigen::Vector3d& _qdot) {
  double theta = _q.norm();

  Eigen::Matrix3d Jdot = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d qss  = math::makeSkewSymmetric(_q);
  Eigen::Matrix3d qss2 = qss*qss;
  Eigen::Matrix3d qdss = math::makeSkewSymmetric(_qdot);
  double ttdot = _q.dot(_qdot);   // theta*thetaDot
  double st = sin(theta);
  double ct = cos(theta);
  double t2 = theta*theta;
  double t3 = t2*theta;
  double t4 = t3*theta;
  double t5 = t4*theta;

  if (theta < EPSILON_EXPMAP_THETA) {
    Jdot = 0.5*qdss + (1.0/6.0)*(qss*qdss + qdss*qss);
    Jdot += (-1.0/12)*ttdot*qss + (-1.0/60)*ttdot*qss2;
  } else {
    Jdot = ((1-ct)/t2)*qdss + ((theta-st)/t3)*(qss*qdss + qdss*qss);
    Jdot += ((theta*st + 2*ct - 2)/t4)*ttdot*qss
            + ((3*st - theta*ct - 2*theta)/t5)*ttdot*qss2;
  }

  return Jdot;
}

Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d& _q, int _qi ) {
  assert(_qi >= 0 && _qi <= 2);

  Eigen::Vector3d qdot = Eigen::Vector3d::Zero();
  qdot[_qi] = 1.0;
  return expMapJacDot(_q, qdot);
}

// Vec3 AdInvTLinear(const SE3& T, const Vec3& v)
// {
//     return Vec3(T(0,0)*v[0] + T(1,0)*v[1] + T(2,0)*v[2],
//                 T(0,1)*v[0] + T(1,1)*v[1] + T(2,1)*v[2],
//                 T(0,2)*v[0] + T(1,2)*v[1] + T(2,2)*v[2]);
// }

// Vec3 ad_Vec3_se3(const Vec3& s1, const se3& s2)
// {
//     Vec3 ret;

//     ret << s2[2]*s1[1] - s2[1]*s1[2],
//            s2[0]*s1[2] - s2[2]*s1[0],
//            s2[1]*s1[0] - s2[0]*s1[1];

//     return ret;
// }

Eigen::Vector3d logMap(const Eigen::Matrix3d& _R) {
  //--------------------------------------------------------------------------
  // T = (R, p) = exp([w, v]), t = ||w||
  // v = beta*p + gamma*w + 1 / 2*cross(p, w)
  //    , beta = t*(1 + cos(t)) / (2*sin(t)), gamma = <w, p>*(1 - beta) / t^2
  //--------------------------------------------------------------------------
//  double theta =
//      std::acos(
//        std::max(
//          std::min(0.5 * (_R(0, 0) + _R(1, 1) + _R(2, 2) - 1.0), 1.0), -1.0));

//  if (theta > constantsd::pi() - DART_EPSILON) {
//    double delta = 0.5 + 0.125*(constantsd::pi() - theta)*(constantsd::pi() - theta);

//    return Eigen::Vector3d(
//          _R(2, 1) > _R(1, 2) ? theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta) :
//                             -theta*sqrt(1.0 + (_R(0, 0) - 1.0)*delta),
//          _R(0, 2) > _R(2, 0) ? theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta) :
//                             -theta*sqrt(1.0 + (_R(1, 1) - 1.0)*delta),
//          _R(1, 0) > _R(0, 1) ? theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta) :
//                             -theta*sqrt(1.0 + (_R(2, 2) - 1.0)*delta));
//  } else {
//    double alpha = 0.0;

//    if (theta > DART_EPSILON)
//      alpha = 0.5*theta / sin(theta);
//    else
//      alpha = 0.5 + constantsd::one_div_12()*theta*theta;

//    return Eigen::Vector3d(alpha*(_R(2, 1) - _R(1, 2)),
//                           alpha*(_R(0, 2) - _R(2, 0)),
//                           alpha*(_R(1, 0) - _R(0, 1)));
//  }

  Eigen::AngleAxisd aa(_R);
  return aa.angle()*aa.axis();
}

Eigen::Vector6d logMap(const Eigen::Isometry3d& _T) {
  //--------------------------------------------------------------------------
  // T = (R, p) = exp([w, v]), t = ||w||
  // v = beta*p + gamma*w + 1 / 2*cross(p, w)
  //    , beta = t*(1 + cos(t)) / (2*sin(t)), gamma = <w, p>*(1 - beta) / t^2
  //--------------------------------------------------------------------------
  double theta =
      std::acos(
        std::max(
          std::min(0.5 * (_T(0, 0) + _T(1, 1) + _T(2, 2) - 1.0), 1.0), -1.0));
  double beta;
  double gamma;
  Eigen::Vector6d ret;

  if (theta > constantsd::pi() - DART_EPSILON) {
    const double c1 = 0.10132118364234;  // 1 / pi^2
    const double c2 = 0.01507440267955;  // 1 / 4 / pi - 2 / pi^3
    const double c3 = 0.00546765085347;  // 3 / pi^4 - 1 / 4 / pi^2

    double phi = constantsd::pi() - theta;
    double delta = 0.5 + 0.125*phi*phi;

    double w[] = { _T(2, 1) > _T(1, 2)
                   ? theta*sqrt(1.0 + (_T(0, 0) - 1.0)*delta)
                   : -theta*sqrt(1.0 + (_T(0, 0) - 1.0)*delta),
                   _T(0, 2) > _T(2, 0)
                   ? theta*sqrt(1.0 + (_T(1, 1) - 1.0)*delta)
                   : -theta*sqrt(1.0 + (_T(1, 1) - 1.0)*delta),
                   _T(1, 0) > _T(0, 1)
                   ? theta*sqrt(1.0 + (_T(2, 2) - 1.0)*delta)
                   : -theta*sqrt(1.0 + (_T(2, 2) - 1.0)*delta) };

    beta = 0.25*theta*(constantsd::pi() - theta);
    gamma = (w[0]*_T(0, 3) + w[1]*_T(1, 3) + w[2]*_T(2, 3))
            * (c1 -  c2*phi + c3*phi*phi);

    ret << w[0],
        w[1],
        w[2],
        beta*_T(0, 3) - 0.5*(w[1]*_T(2, 3) - w[2]*_T(1, 3)) + gamma*w[0],
        beta*_T(1, 3) - 0.5*(w[2]*_T(0, 3) - w[0]*_T(2, 3)) + gamma*w[1],
        beta*_T(2, 3) - 0.5*(w[0]*_T(1, 3) - w[1]*_T(0, 3)) + gamma*w[2];
  } else {
    double alpha;
    if (theta > DART_EPSILON) {
      alpha = 0.5*theta / sin(theta);
      beta = (1.0 + cos(theta))*alpha;
      gamma = (1.0 - beta) / theta / theta;
    } else {
      alpha = 0.5 + 1.0/12.0*theta*theta;
      beta = 1.0 - 1.0/12.0*theta*theta;
      gamma = 1.0/12.0 + 1.0/720.0*theta*theta;
    }

    double w[] = { alpha*(_T(2, 1) - _T(1, 2)),
                   alpha*(_T(0, 2) - _T(2, 0)),
                   alpha*(_T(1, 0) - _T(0, 1)) };
    gamma *= w[0]*_T(0, 3) + w[1]*_T(1, 3) + w[2]*_T(2, 3);

    ret << w[0],
        w[1],
        w[2],
        beta*_T(0, 3) + 0.5*(w[2]*_T(1, 3) - w[1]*_T(2, 3)) + gamma*w[0],
        beta*_T(1, 3) + 0.5*(w[0]*_T(2, 3) - w[2]*_T(0, 3)) + gamma*w[1],
        beta*_T(2, 3) + 0.5*(w[1]*_T(0, 3) - w[0]*_T(1, 3)) + gamma*w[2];
  }

  return ret;
}

// res = T * s * Inv(T)
Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V) {
  //--------------------------------------------------------------------------
  // w' = R*w
  // v' = p x R*w + R*v
  //--------------------------------------------------------------------------
  Eigen::Vector6d res;
  res.head<3>().noalias() = _T.linear() * _V.head<3>();
  res.tail<3>().noalias() = _T.linear() * _V.tail<3>() +
                            _T.translation().cross(res.head<3>());
  return res;
}

//==============================================================================
Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T)
{
  Eigen::Matrix6d AdT;

  AdT.topLeftCorner<3, 3>() = T.linear();
  AdT.topRightCorner<3, 3>().setZero();
  AdT.bottomLeftCorner<3, 3>()
      = makeSkewSymmetric(T.translation()) * T.linear();
  AdT.bottomRightCorner<3, 3>() = T.linear();

  return AdT;
}

Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V) {
  //--------------------------------------------------------------------------
  // w' = R*w
  // v' = R*v
  //--------------------------------------------------------------------------
  Eigen::Vector6d res;
  res.head<3>().noalias() = _T.linear() * _V.head<3>();
  res.tail<3>().noalias() = _T.linear() * _V.tail<3>();
  return res;
}

Eigen::Vector6d AdTAngular(const Eigen::Isometry3d& _T,
                           const Eigen::Vector3d& _w) {
  //--------------------------------------------------------------------------
  // w' = R*w
  // v' = p x R*w
  //--------------------------------------------------------------------------
  Eigen::Vector6d res;
  res.head<3>().noalias() = _T.linear() * _w;
  res.tail<3>() = _T.translation().cross(res.head<3>());
  return res;
}

Eigen::Vector6d AdTLinear(const Eigen::Isometry3d& _T,
                          const Eigen::Vector3d& _v) {
  //--------------------------------------------------------------------------
  // w' = 0
  // v' = R*v
  //--------------------------------------------------------------------------
  Eigen::Vector6d res = Eigen::Vector6d::Zero();
  res.tail<3>().noalias() = _T.linear() * _v;
  return res;
}

// se3 AdP(const Vec3& p, const se3& s)
// {
//  //--------------------------------------------------------------------------
//  // w' = w
//  // v' = p x w + v
//  //--------------------------------------------------------------------------
//  se3 ret;
//  ret << s[0],
//      s[1],
//      s[2],
//      p[1]*s[2] - p[2]*s[1] + s[3],
//      p[2]*s[0] - p[0]*s[2] + s[4],
//      p[0]*s[1] - p[1]*s[0] + s[5];

//  return ret;
// }

// Jacobian AdRJac(const SE3& T, const Jacobian& J)
// {
//     Jacobian AdTJ(6,J.cols());

//     for (int i = 0; i < J.cols(); ++i)
//     {
//         AdTJ.col(i) = math::AdR(T, J.col(i));
//     }

//     return AdTJ;
// }

// re = Inv(T)*s*T
Eigen::Vector6d AdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V) {
  Eigen::Vector6d res;
  res.head<3>().noalias() = _T.linear().transpose() * _V.head<3>();
  res.tail<3>().noalias() =
      _T.linear().transpose()
      * (_V.tail<3>() + _V.head<3>().cross(_T.translation()));
  return res;
}

// se3 AdInvR(const SE3& T, const se3& s)
// {
//     se3 ret;

//     ret << T(0,0)*s[0] + T(1,0)*s[1] + T(2,0)*s[2],
//                 T(0,1)*s[0] + T(1,1)*s[1] + T(2,1)*s[2],
//                 T(0,2)*s[0] + T(1,2)*s[1] + T(2,2)*s[2],
//                 T(0,0)*s[3] + T(1,0)*s[4] + T(2,0)*s[5],
//                 T(0,1)*s[3] + T(1,1)*s[4] + T(2,1)*s[5],
//                 T(0,2)*s[3] + T(1,2)*s[4] + T(2,2)*s[5];

//     return ret;
// }

Eigen::Vector6d AdInvRLinear(const Eigen::Isometry3d& _T,
                             const Eigen::Vector3d& _v) {
  Eigen::Vector6d res = Eigen::Vector6d::Zero();
  res.tail<3>().noalias() = _T.linear().transpose() * _v;
  return res;
}

Eigen::Vector6d ad(const Eigen::Vector6d& _X, const Eigen::Vector6d& _Y) {
  //--------------------------------------------------------------------------
  // ad(s1, s2) = | [w1]    0 | | w2 |
  //              | [v1] [w1] | | v2 |
  //
  //            = |          [w1]w2 |
  //              | [v1]w2 + [w1]v2 |
  //--------------------------------------------------------------------------
  Eigen::Vector6d res;
  res.head<3>() = _X.head<3>().cross(_Y.head<3>());
  res.tail<3>() = _X.head<3>().cross(_Y.tail<3>()) +
                  _X.tail<3>().cross(_Y.head<3>());
  return res;
}

Eigen::Vector6d dAdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F) {
  Eigen::Vector6d res;
  res.head<3>().noalias() =
      _T.linear().transpose()
      * (_F.head<3>() + _F.tail<3>().cross(_T.translation()));
  res.tail<3>().noalias() = _T.linear().transpose() * _F.tail<3>();
  return res;
}

// dse3 dAdTLinear(const SE3& T, const Vec3& v)
// {
//     dse3 ret;
//     double tmp[3] = { - T(1,3)*v[2] + T(2,3)*v[1],
//                       - T(2,3)*v[0] + T(0,3)*v[2],
//                       - T(0,3)*v[1] + T(1,3)*v[0] };
//     ret << T(0,0)*tmp[0] + T(1,0)*tmp[1] + T(2,0)*tmp[2],
//                 T(0,1)*tmp[0] + T(1,1)*tmp[1] + T(2,1)*tmp[2],
//                 T(0,2)*tmp[0] + T(1,2)*tmp[1] + T(2,2)*tmp[2],
//                 T(0,0)*v[0] + T(1,0)*v[1] + T(2,0)*v[2],
//                 T(0,1)*v[0] + T(1,1)*v[1] + T(2,1)*v[2],
//                 T(0,2)*v[0] + T(1,2)*v[1] + T(2,2)*v[2];

//     return ret;
// }

Eigen::Vector6d dAdInvT(const Eigen::Isometry3d& _T,
                        const Eigen::Vector6d& _F) {
  Eigen::Vector6d res;
  res.tail<3>().noalias() = _T.linear() * _F.tail<3>();
  res.head<3>().noalias() = _T.linear() * _F.head<3>();
  res.head<3>() += _T.translation().cross(res.tail<3>());
  return res;
}

Eigen::Vector6d dAdInvR(const Eigen::Isometry3d& _T,
                        const Eigen::Vector6d& _F) {
  Eigen::Vector6d res;
  res.head<3>().noalias() = _T.linear() * _F.head<3>();
  res.tail<3>().noalias() = _T.linear() * _F.tail<3>();
  return res;
}

// dse3 dAdInvPLinear(const Vec3& p, const Vec3& f)
// {
//     dse3 ret;

//     ret << p[1]*f[2] - p[2]*f[1],
//                 p[2]*f[0] - p[0]*f[2],
//                 p[0]*f[1] - p[1]*f[0],
//                 f[0],
//                 f[1],
//                 f[2];

//     return ret;
// }

// Reference:
// http://www.geometrictools.com/LibMathematics/Algebra/Wm5Matrix3.inl
Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                                -+
  // | r00 r01 r02 |   |  cy      sy*sx1               sy*cx1             |
  // | r10 r11 r12 | = |  sy*sx0  cx0*cx1-cy*sx0*sx1  -cy*cx1*sx0-cx0*sx1 |
  // | r20 r21 r22 |   | -sy*cx0  cx1*sx0+cy*cx0*sx1   cy*cx0*cx1-sx0*sx1 |
  // +-           -+   +-                                                -+

  Eigen::Matrix3d ret;

  double cx0 = cos(_angle(0));
  double sx0 = sin(_angle(0));
  double cy  = cos(_angle(1));
  double sy  = sin(_angle(1));
  double cx1 = cos(_angle(2));
  double sx1 = sin(_angle(2));

  ret(0, 0) =  cy;
  ret(1, 0) =  sy*sx0;
  ret(2, 0) = -sy*cx0;

  ret(0, 1) = sy*sx1;
  ret(1, 1) = cx0*cx1-cy*sx0*sx1;
  ret(2, 1) = cx1*sx0+cy*cx0*sx1;

  ret(0, 2) =  sy*cx1;
  ret(1, 2) = -cy*cx1*sx0-cx0*sx1;
  ret(2, 2) =  cy*cx0*cx1-sx0*sx1;

  return ret;
}

Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                        -+
  // | r00 r01 r02 |   |  cy*cz           -cy*sz            sy    |
  // | r10 r11 r12 | = |  cz*sx*sy+cx*sz   cx*cz-sx*sy*sz  -cy*sx |
  // | r20 r21 r22 |   | -cx*cz*sy+sx*sz   cz*sx+cx*sy*sz   cx*cy |
  // +-           -+   +-                                        -+

  Eigen::Matrix3d ret;

  double cx = cos(_angle[0]);
  double sx = sin(_angle[0]);
  double cy = cos(_angle[1]);
  double sy = sin(_angle[1]);
  double cz = cos(_angle[2]);
  double sz = sin(_angle[2]);

  ret(0, 0) = cy*cz;
  ret(1, 0) = cx*sz + cz*sx*sy;
  ret(2, 0) = sx*sz - cx*cz*sy;

  ret(0, 1) = -cy*sz;
  ret(1, 1) =  cx*cz - sx*sy*sz;
  ret(2, 1) =  cz*sx + cx*sy*sz;

  ret(0, 2) =  sy;
  ret(1, 2) = -cy*sx;
  ret(2, 2) =  cx*cy;

  return ret;
}

Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                                -+
  // | r00 r01 r02 |   | cz      -sz*cx1               sz*sx1             |
  // | r10 r11 r12 | = | sz*cx0   cz*cx0*cx1-sx0*sx1  -cx1*sx0-cz*cx0*sx1 |
  // | r20 r21 r22 |   | sz*sx0   cz*cx1*sx0+cx0*sx1   cx0*cx1-cz*sx0*sx1 |
  // +-           -+   +-                                                -+

  Eigen::Matrix3d ret;

  double cx0 = cos(_angle(0));
  double sx0 = sin(_angle(0));
  double cz  = cos(_angle(1));
  double sz  = sin(_angle(1));
  double cx1 = cos(_angle(2));
  double sx1 = sin(_angle(2));

  ret(0, 0) = cz;
  ret(1, 0) = sz*cx0;
  ret(2, 0) = sz*sx0;

  ret(0, 1) = -sz*cx1;
  ret(1, 1) =  cz*cx0*cx1-sx0*sx1;
  ret(2, 1) =  cz*cx1*sx0+cx0*sx1;

  ret(0, 2) =  sz*sx1;
  ret(1, 2) = -cx1*sx0-cz*cx0*sx1;
  ret(2, 2) =  cx0*cx1-cz*sx0*sx1;

  return ret;
}

Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                        -+
  // | r00 r01 r02 |   |  cy*cz           -sz      cz*sy          |
  // | r10 r11 r12 | = |  sx*sy+cx*cy*sz   cx*cz  -cy*sx+cx*sy*sz |
  // | r20 r21 r22 |   | -cx*sy+cy*sx*sz   cz*sx   cx*cy+sx*sy*sz |
  // +-           -+   +-                                        -+

  Eigen::Matrix3d ret;

  double cx = cos(_angle(0));
  double sx = sin(_angle(0));
  double cz = cos(_angle(1));
  double sz = sin(_angle(1));
  double cy = cos(_angle(2));
  double sy = sin(_angle(2));

  ret(0, 0) =  cy*cz;
  ret(1, 0) =  sx*sy+cx*cy*sz;
  ret(2, 0) = -cx*sy+cy*sx*sz;

  ret(0, 1) = -sz;
  ret(1, 1) =  cx*cz;
  ret(2, 1) =  cz*sx;

  ret(0, 2) =  cz*sy;
  ret(1, 2) = -cy*sx+cx*sy*sz;
  ret(2, 2) =  cx*cy+sx*sy*sz;

  return ret;
}

Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                                -+
  // | r00 r01 r02 |   |  cy0*cy1-cx*sy0*sy1  sx*sy0   cx*cy1*sy0+cy0*sy1 |
  // | r10 r11 r12 | = |  sx*sy1              cx      -sx*cy1             |
  // | r20 r21 r22 |   | -cy1*sy0-cx*cy0*sy1  sx*cy0   cx*cy0*cy1-sy0*sy1 |
  // +-           -+   +-                                                -+

  Eigen::Matrix3d ret;

  double cy0 = cos(_angle(0));
  double sy0 = sin(_angle(0));
  double cx  = cos(_angle(1));
  double sx  = sin(_angle(1));
  double cy1 = cos(_angle(2));
  double sy1 = sin(_angle(2));

  ret(0, 0) =  cy0*cy1-cx*sy0*sy1;
  ret(1, 0) =  sx*sy1;
  ret(2, 0) = -cy1*sy0-cx*cy0*sy1;

  ret(0, 1) = sx*sy0;
  ret(1, 1) = cx;
  ret(2, 1) = sx*cy0;

  ret(0, 2) =  cx*cy1*sy0+cy0*sy1;
  ret(1, 2) = -sx*cy1;
  ret(2, 2) =  cx*cy0*cy1-sy0*sy1;

  return ret;
}

Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                       -+
  // | r00 r01 r02 |   |  cy*cz+sx*sy*sz  cz*sx*sy-cy*sz   cx*sy |
  // | r10 r11 r12 | = |  cx*sz           cx*cz           -sx    |
  // | r20 r21 r22 |   | -cz*sy+cy*sx*sz  cy*cz*sx+sy*sz   cx*cy |
  // +-           -+   +-                                       -+

  Eigen::Matrix3d ret;

  double cy = cos(_angle(0));
  double sy = sin(_angle(0));
  double cx = cos(_angle(1));
  double sx = sin(_angle(1));
  double cz = cos(_angle(2));
  double sz = sin(_angle(2));

  ret(0, 0) =  cy*cz+sx*sy*sz;  ret(0, 1) = cz*sx*sy-cy*sz;  ret(0, 2) =  cx*sy;
  ret(1, 0) =  cx*sz;           ret(1, 1) = cx*cz;           ret(1, 2) = -sx;
  ret(2, 0) = -cz*sy+cy*sx*sz;  ret(2, 1) = cy*cz*sx+sy*sz;  ret(2, 2) =  cx*cy;

  return ret;
}

Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                       -+
  // | r00 r01 r02 |   |  cy*cz  sx*sy-cx*cy*sz   cx*sy+cy*sx*sz |
  // | r10 r11 r12 | = |  sz     cx*cz           -cz*sx          |
  // | r20 r21 r22 |   | -cz*sy  cy*sx+cx*sy*sz   cx*cy-sx*sy*sz |
  // +-           -+   +-                                       -+

  Eigen::Matrix3d ret;

  double cy = cos(_angle(0));
  double sy = sin(_angle(0));
  double cz = cos(_angle(1));
  double sz = sin(_angle(1));
  double cx = cos(_angle(2));
  double sx = sin(_angle(2));

  ret(0, 0) =  cy*cz;  ret(0, 1) = sx*sy-cx*cy*sz;  ret(0, 2) =  cx*sy+cy*sx*sz;
  ret(1, 0) =  sz;     ret(1, 1) = cx*cz;           ret(1, 2) = -cz*sx;
  ret(2, 0) = -cz*sy;  ret(2, 1) = cy*sx+cx*sy*sz;  ret(2, 2) =  cx*cy-sx*sy*sz;

  return ret;
}

Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                                -+
  // | r00 r01 r02 |   |  cz*cy0*cy1-sy0*sy1  -sz*cy0  cy1*sy0+cz*cy0*sy1 |
  // | r10 r11 r12 | = |  sz*cy1               cz      sz*sy1             |
  // | r20 r21 r22 |   | -cz*cy1*sy0-cy0*sy1   sz*sy0  cy0*cy1-cz*sy0*sy1 |
  // +-           -+   +-                                                -+

  Eigen::Matrix3d ret;

  double cy0 = cos(_angle(0));
  double sy0 = sin(_angle(0));
  double cz  = cos(_angle(1));
  double sz  = sin(_angle(1));
  double cy1 = cos(_angle(2));
  double sy1 = sin(_angle(2));

  ret(0, 0) =  cz*cy0*cy1-sy0*sy1;
  ret(1, 0) =  sz*cy1;
  ret(2, 0) = -cz*cy1*sy0-cy0*sy1;

  ret(0, 1) = -sz*cy0;
  ret(1, 1) =  cz;
  ret(2, 1) =  sz*sy0;

  ret(0, 2) = cy1*sy0+cz*cy0*sy1;
  ret(1, 2) = sz*sy1;
  ret(2, 2) = cy0*cy1-cz*sy0*sy1;

  return ret;
}

Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                        -+
  // | r00 r01 r02 |   |  cy*cz-sx*sy*sz  -cx*sz   cz*sy+cy*sx*sz |
  // | r10 r11 r12 | = |  cz*sx*sy+cy*sz   cx*cz  -cy*cz*sx+sy*sz |
  // | r20 r21 r22 |   | -cx*sy            sx      cx*cy          |
  // +-           -+   +-                                        -+

  Eigen::Matrix3d ret;

  double cz = cos(_angle(0));
  double sz = sin(_angle(0));
  double cx = cos(_angle(1));
  double sx = sin(_angle(1));
  double cy = cos(_angle(2));
  double sy = sin(_angle(2));

  ret(0, 0) =  cy*cz-sx*sy*sz;
  ret(1, 0) =  cz*sx*sy+cy*sz;
  ret(2, 0) = -cx*sy;

  ret(0, 1) = -cx*sz;
  ret(1, 1) =  cx*cz;
  ret(2, 1) =  sx;

  ret(0, 2) =  cz*sy+cy*sx*sz;
  ret(1, 2) = -cy*cz*sx+sy*sz;
  ret(2, 2) =  cx*cy;

  return ret;
}

Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                      -+
  // | r00 r01 r02 |   |  cy*cz  cz*sx*sy-cx*sz  cx*cz*sy+sx*sz |
  // | r10 r11 r12 | = |  cy*sz  cx*cz+sx*sy*sz -cz*sx+cx*sy*sz |
  // | r20 r21 r22 |   | -sy     cy*sx           cx*cy          |
  // +-           -+   +-                                      -+

  Eigen::Matrix3d ret;

  double cz = cos(_angle[0]);
  double sz = sin(_angle[0]);
  double cy = cos(_angle[1]);
  double sy = sin(_angle[1]);
  double cx = cos(_angle[2]);
  double sx = sin(_angle[2]);

  ret(0, 0) =  cz*cy;
  ret(1, 0) =  sz*cy;
  ret(2, 0) = -sy;

  ret(0, 1) = cz*sy*sx - sz*cx;
  ret(1, 1) = sz*sy*sx + cz*cx;
  ret(2, 1) = cy*sx;

  ret(0, 2) = cz*sy*cx + sz*sx;
  ret(1, 2) = sz*sy*cx - cz*sx;
  ret(2, 2) = cy*cx;

  return ret;
}

Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                                -+
  // | r00 r01 r02 |   | cz0*cz1-cx*sz0*sz1  -cx*cz1*sz0-cz0*sz1   sx*sz0 |
  // | r10 r11 r12 | = | cz1*sz0+cx*cz0*sz1   cx*cz0*cz1-sz0*sz1  -sz*cz0 |
  // | r20 r21 r22 |   | sx*sz1               sx*cz1               cx     |
  // +-           -+   +-                                                -+

  Eigen::Matrix3d ret;

  double cz0 = cos(_angle(0));
  double sz0 = sin(_angle(0));
  double cx  = cos(_angle(1));
  double sx  = sin(_angle(1));
  double cz1 = cos(_angle(2));
  double sz1 = sin(_angle(2));

  ret(0, 0) = cz0*cz1-cx*sz0*sz1;
  ret(1, 0) = cz1*sz0+cx*cz0*sz1;
  ret(2, 0) = sx*sz1;

  ret(0, 1) = -cx*cz1*sz0-cz0*sz1;
  ret(1, 1) =  cx*cz0*cz1-sz0*sz1;
  ret(2, 1) =  sx*cz1;

  ret(0, 2) =  sx*sz0;
  ret(1, 2) = -sx*cz0;
  ret(2, 2) =  cx;

  return ret;
}

Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle) {
  // +-           -+   +-                                                -+
  // | r00 r01 r02 |   |  cy*cz0*cz1-sz0*sz1  -cz1*sz0-cy*cz0*sz1  sy*cz0 |
  // | r10 r11 r12 | = |  cy*cz1*sz0+cz0*sz1   cz0*cz1-cy*sz0*sz1  sy*sz0 |
  // | r20 r21 r22 |   | -sy*cz1               sy*sz1              cy     |
  // +-           -+   +-                                                -+

  Eigen::Matrix3d ret = Eigen::Matrix3d::Identity();

  double cz0 = cos(_angle[0]);
  double sz0 = sin(_angle[0]);
  double cy = cos(_angle[1]);
  double sy = sin(_angle[1]);
  double cz1 = cos(_angle[2]);
  double sz1 = sin(_angle[2]);

  ret(0, 0) =  cz0*cy*cz1 - sz0*sz1;
  ret(1, 0) =  sz0*cy*cz1 + cz0*sz1;
  ret(2, 0) = -sy*cz1;

  ret(0, 1) = -cz0*cy*sz1 - sz0*cz1;
  ret(1, 1) = cz0*cz1 - sz0*cy*sz1;
  ret(2, 1) = sy*sz1;

  ret(0, 2) = cz0*sy;
  ret(1, 2) = sz0*sy;
  ret(2, 2) = cy;

  return ret;
}

// R = Exp(w)
// p = sin(t) / t*v + (t - sin(t)) / t^3*<w, v>*w + (1 - cos(t)) / t^2*(w X v)
// , when S = (w, v), t = |w|
Eigen::Isometry3d expMap(const Eigen::Vector6d& _S) {
  Eigen::Isometry3d ret = Eigen::Isometry3d::Identity();
  double s2[] = { _S[0]*_S[0], _S[1]*_S[1], _S[2]*_S[2] };
  double s3[] = { _S[0]*_S[1], _S[1]*_S[2], _S[2]*_S[0] };
  double theta = sqrt(s2[0] + s2[1] + s2[2]);
  double cos_t = cos(theta), alpha, beta, gamma;

  if (theta > DART_EPSILON) {
    double sin_t = sin(theta);
    alpha = sin_t / theta;
    beta = (1.0 - cos_t) / theta / theta;
    gamma = (_S[0]*_S[3] + _S[1]*_S[4] + _S[2]*_S[5])
            * (theta - sin_t) / theta / theta / theta;
  } else {
    alpha = 1.0 - theta*theta/6.0;
    beta = 0.5 - theta*theta/24.0;
    gamma = (_S[0]*_S[3] + _S[1]*_S[4] + _S[2]*_S[5])/6.0 - theta*theta/120.0;
  }

  ret(0, 0) = beta*s2[0] + cos_t;
  ret(1, 0) = beta*s3[0] + alpha*_S[2];
  ret(2, 0) = beta*s3[2] - alpha*_S[1];

  ret(0, 1) = beta*s3[0] - alpha*_S[2];
  ret(1, 1) = beta*s2[1] + cos_t;
  ret(2, 1) = beta*s3[1] + alpha*_S[0];

  ret(0, 2) = beta*s3[2] + alpha*_S[1];
  ret(1, 2) = beta*s3[1] - alpha*_S[0];
  ret(2, 2) = beta*s2[2] + cos_t;

  ret(0, 3) = alpha*_S[3] + beta*(_S[1]*_S[5] - _S[2]*_S[4]) + gamma*_S[0];
  ret(1, 3) = alpha*_S[4] + beta*(_S[2]*_S[3] - _S[0]*_S[5]) + gamma*_S[1];
  ret(2, 3) = alpha*_S[5] + beta*(_S[0]*_S[4] - _S[1]*_S[3]) + gamma*_S[2];

  return ret;
}

// I + sin(t) / t*[S] + (1 - cos(t)) / t^2*[S]^2, where t = |S|
Eigen::Isometry3d expAngular(const Eigen::Vector3d& _s) {
  Eigen::Isometry3d ret = Eigen::Isometry3d::Identity();
  double s2[] = { _s[0]*_s[0], _s[1]*_s[1], _s[2]*_s[2] };
  double s3[] = { _s[0]*_s[1], _s[1]*_s[2], _s[2]*_s[0] };
  double theta = sqrt(s2[0] + s2[1] + s2[2]);
  double cos_t = cos(theta);
  double alpha = 0.0;
  double beta = 0.0;

  if (theta > DART_EPSILON) {
    alpha = sin(theta) / theta;
    beta = (1.0 - cos_t) / theta / theta;
  } else {
    alpha = 1.0 - theta*theta/6.0;
    beta = 0.5 - theta*theta/24.0;
  }

  ret(0, 0) = beta*s2[0] + cos_t;
  ret(1, 0) = beta*s3[0] + alpha*_s[2];
  ret(2, 0) = beta*s3[2] - alpha*_s[1];

  ret(0, 1) = beta*s3[0] - alpha*_s[2];
  ret(1, 1) = beta*s2[1] + cos_t;
  ret(2, 1) = beta*s3[1] + alpha*_s[0];

  ret(0, 2) = beta*s3[2] + alpha*_s[1];
  ret(1, 2) = beta*s3[1] - alpha*_s[0];
  ret(2, 2) = beta*s2[2] + cos_t;

  return ret;
}

// SE3 Normalize(const SE3& T)
// {
//    SE3 ret = SE3::Identity();
//    double idet = 1.0 / (T(0,0)*(T(1,1)*T(2,2) - T(2,1)*T(1,2)) +
//                              T(0,1)*(T(2,0)*T(1,2) - T(1,0)*T(2,2)) +
//                              T(0,2)*(T(1,0)*T(2,1) - T(2,0)*T(1,1)));

//    ret(0,0) = 1.0_2*(T(0,0) + idet*(T(1,1)*T(2,2) - T(2,1)*T(1,2)));
//    ret(0,1) = 1.0_2*(T(0,1) + idet*(T(2,0)*T(1,2) - T(1,0)*T(2,2)));
//    ret(0,2) = 1.0_2*(T(0,2) + idet*(T(1,0)*T(2,1) - T(2,0)*T(1,1)));
//    ret(0,3) = T(0,3);

//    ret(1,0) = 1.0_2*(T(1,0) + idet*(T(2,1)*T(0,2) - T(0,1)*T(2,2)));
//    ret(1,1) = 1.0_2*(T(1,1) + idet*(T(0,0)*T(2,2) - T(2,0)*T(0,2)));
//    ret(1,2) = 1.0_2*(T(1,2) + idet*(T(2,0)*T(0,1) - T(0,0)*T(2,1)));
//    ret(1,3) = T(1,3);

//    ret(2,0) = 1.0_2*(T(2,0) + idet*(T(0,1)*T(1,2) - T(1,1)*T(0,2)));
//    ret(2,1) = 1.0_2*(T(2,1) + idet*(T(1,0)*T(0,2) - T(0,0)*T(1,2)));
//    ret(2,2) = 1.0_2*(T(2,2) + idet*(T(0,0)*T(1,1) - T(1,0)*T(0,1)));
//    ret(2,3) = T(2,3);

//    return ret;
// }

// Axis Reparameterize(const Axis& s)
// {
//  double theta = std::sqrt(s[0]*s[0] + s[1]*s[1] + s[2]*s[2]);
//  double eta = theta < LIE_EPS
//               ? 1.0
//               : 1.0 - (double)((int)(theta / M_PI + 1.0) / 2)*M_2PI / theta;

//  return eta*s;
// }

// Axis AdInvTAngular(const SE3& T, const Axis& v)
// {
//    return Axis(T(0,0)*v[0] + T(1,0)*v[1] + T(2,0)*v[2],
//                T(0,1)*v[0] + T(1,1)*v[1] + T(2,1)*v[2],
//                T(0,2)*v[0] + T(1,2)*v[1] + T(2,2)*v[2]);
// }

// Axis ad(const Axis& s1, const se3& s2)
// {
//    return Axis(s2[2]*s1[1] - s2[1]*s1[2],
//                s2[0]*s1[2] - s2[2]*s1[0],
//                s2[1]*s1[0] - s2[0]*s1[1]);
// }

// Axis ad_Axis_Axis(const Axis& s1, const Axis& s2)
// {
//    return Axis(s2[2]*s1[1] - s2[1]*s1[2],
//                s2[0]*s1[2] - s2[2]*s1[0],
//                s2[1]*s1[0] - s2[0]*s1[1]);
// }

Eigen::Vector6d dad(const Eigen::Vector6d& _s, const Eigen::Vector6d& _t) {
  Eigen::Vector6d res;
  res.head<3>() = _t.head<3>().cross(_s.head<3>())
                  + _t.tail<3>().cross(_s.tail<3>());
  res.tail<3>() = _t.tail<3>().cross(_s.head<3>());
  return res;
}

Inertia transformInertia(const Eigen::Isometry3d& _T, const Inertia& _I) {
  // operation count: multiplication = 186, addition = 117, subtract = 21

  Inertia ret = Inertia::Identity();

  double d0 = _I(0, 3) + _T(2, 3) * _I(3, 4) - _T(1, 3) * _I(3, 5);
  double d1 = _I(1, 3) - _T(2, 3) * _I(3, 3) + _T(0, 3) * _I(3, 5);
  double d2 = _I(2, 3) + _T(1, 3) * _I(3, 3) - _T(0, 3) * _I(3, 4);
  double d3 = _I(0, 4) + _T(2, 3) * _I(4, 4) - _T(1, 3) * _I(4, 5);
  double d4 = _I(1, 4) - _T(2, 3) * _I(3, 4) + _T(0, 3) * _I(4, 5);
  double d5 = _I(2, 4) + _T(1, 3) * _I(3, 4) - _T(0, 3) * _I(4, 4);
  double d6 = _I(0, 5) + _T(2, 3) * _I(4, 5) - _T(1, 3) * _I(5, 5);
  double d7 = _I(1, 5) - _T(2, 3) * _I(3, 5) + _T(0, 3) * _I(5, 5);
  double d8 = _I(2, 5) + _T(1, 3) * _I(3, 5) - _T(0, 3) * _I(4, 5);
  double e0 = _I(0, 0) + _T(2, 3) * _I(0, 4) - _T(1, 3) * _I(0, 5)
              + d3 * _T(2, 3) - d6 * _T(1, 3);
  double e3 = _I(0, 1) + _T(2, 3) * _I(1, 4) - _T(1, 3) * _I(1, 5)
              - d0 * _T(2, 3) + d6 * _T(0, 3);
  double e4 = _I(1, 1) - _T(2, 3) * _I(1, 3) + _T(0, 3) * _I(1, 5)
              - d1 * _T(2, 3) + d7 * _T(0, 3);
  double e6 = _I(0, 2) + _T(2, 3) * _I(2, 4) - _T(1, 3) * _I(2, 5)
              + d0 * _T(1, 3) - d3 * _T(0, 3);
  double e7 = _I(1, 2) - _T(2, 3) * _I(2, 3) + _T(0, 3) * _I(2, 5)
              + d1 * _T(1, 3) - d4 * _T(0, 3);
  double e8 = _I(2, 2) + _T(1, 3) * _I(2, 3) - _T(0, 3) * _I(2, 4)
              + d2 * _T(1, 3) - d5 * _T(0, 3);
  double f0 = _T(0, 0) * e0 + _T(1, 0) * e3 + _T(2, 0) * e6;
  double f1 = _T(0, 0) * e3 + _T(1, 0) * e4 + _T(2, 0) * e7;
  double f2 = _T(0, 0) * e6 + _T(1, 0) * e7 + _T(2, 0) * e8;
  double f3 = _T(0, 0) * d0 + _T(1, 0) * d1 + _T(2, 0) * d2;
  double f4 = _T(0, 0) * d3 + _T(1, 0) * d4 + _T(2, 0) * d5;
  double f5 = _T(0, 0) * d6 + _T(1, 0) * d7 + _T(2, 0) * d8;
  double f6 = _T(0, 1) * e0 + _T(1, 1) * e3 + _T(2, 1) * e6;
  double f7 = _T(0, 1) * e3 + _T(1, 1) * e4 + _T(2, 1) * e7;
  double f8 = _T(0, 1) * e6 + _T(1, 1) * e7 + _T(2, 1) * e8;
  double g0 = _T(0, 1) * d0 + _T(1, 1) * d1 + _T(2, 1) * d2;
  double g1 = _T(0, 1) * d3 + _T(1, 1) * d4 + _T(2, 1) * d5;
  double g2 = _T(0, 1) * d6 + _T(1, 1) * d7 + _T(2, 1) * d8;
  double g3 = _T(0, 2) * d0 + _T(1, 2) * d1 + _T(2, 2) * d2;
  double g4 = _T(0, 2) * d3 + _T(1, 2) * d4 + _T(2, 2) * d5;
  double g5 = _T(0, 2) * d6 + _T(1, 2) * d7 + _T(2, 2) * d8;
  double h0 = _T(0, 0) * _I(3, 3) + _T(1, 0) * _I(3, 4) + _T(2, 0) * _I(3, 5);
  double h1 = _T(0, 0) * _I(3, 4) + _T(1, 0) * _I(4, 4) + _T(2, 0) * _I(4, 5);
  double h2 = _T(0, 0) * _I(3, 5) + _T(1, 0) * _I(4, 5) + _T(2, 0) * _I(5, 5);
  double h3 = _T(0, 1) * _I(3, 3) + _T(1, 1) * _I(3, 4) + _T(2, 1) * _I(3, 5);
  double h4 = _T(0, 1) * _I(3, 4) + _T(1, 1) * _I(4, 4) + _T(2, 1) * _I(4, 5);
  double h5 = _T(0, 1) * _I(3, 5) + _T(1, 1) * _I(4, 5) + _T(2, 1) * _I(5, 5);

  ret(0, 0) = f0 * _T(0, 0) + f1 * _T(1, 0) + f2 * _T(2, 0);
  ret(0, 1) = f0 * _T(0, 1) + f1 * _T(1, 1) + f2 * _T(2, 1);
  ret(0, 2) = f0 * _T(0, 2) + f1 * _T(1, 2) + f2 * _T(2, 2);
  ret(0, 3) = f3 * _T(0, 0) + f4 * _T(1, 0) + f5 * _T(2, 0);
  ret(0, 4) = f3 * _T(0, 1) + f4 * _T(1, 1) + f5 * _T(2, 1);
  ret(0, 5) = f3 * _T(0, 2) + f4 * _T(1, 2) + f5 * _T(2, 2);
  ret(1, 1) = f6 * _T(0, 1) + f7 * _T(1, 1) + f8 * _T(2, 1);
  ret(1, 2) = f6 * _T(0, 2) + f7 * _T(1, 2) + f8 * _T(2, 2);
  ret(1, 3) = g0 * _T(0, 0) + g1 * _T(1, 0) + g2 * _T(2, 0);
  ret(1, 4) = g0 * _T(0, 1) + g1 * _T(1, 1) + g2 * _T(2, 1);
  ret(1, 5) = g0 * _T(0, 2) + g1 * _T(1, 2) + g2 * _T(2, 2);
  ret(2, 2) = (_T(0, 2) * e0 + _T(1, 2) * e3 + _T(2, 2) * e6) * _T(0, 2)
              + (_T(0, 2) * e3 + _T(1, 2) * e4 + _T(2, 2) * e7) * _T(1, 2)
              + (_T(0, 2) * e6 + _T(1, 2) * e7 + _T(2, 2) * e8) * _T(2, 2);
  ret(2, 3) = g3 * _T(0, 0) + g4 * _T(1, 0) + g5 * _T(2, 0);
  ret(2, 4) = g3 * _T(0, 1) + g4 * _T(1, 1) + g5 * _T(2, 1);
  ret(2, 5) = g3 * _T(0, 2) + g4 * _T(1, 2) + g5 * _T(2, 2);
  ret(3, 3) = h0 * _T(0, 0) + h1 * _T(1, 0) + h2 * _T(2, 0);
  ret(3, 4) = h0 * _T(0, 1) + h1 * _T(1, 1) + h2 * _T(2, 1);
  ret(3, 5) = h0 * _T(0, 2) + h1 * _T(1, 2) + h2 * _T(2, 2);
  ret(4, 4) = h3 * _T(0, 1) + h4 * _T(1, 1) + h5 * _T(2, 1);
  ret(4, 5) = h3 * _T(0, 2) + h4 * _T(1, 2) + h5 * _T(2, 2);
  ret(5, 5) =
      (_T(0, 2) * _I(3, 3) + _T(1, 2) * _I(3, 4) + _T(2, 2) * _I(3, 5))
      * _T(0, 2)
      + (_T(0, 2) * _I(3, 4) + _T(1, 2) * _I(4, 4) + _T(2, 2) * _I(4, 5))
      * _T(1, 2)
      + (_T(0, 2) * _I(3, 5) + _T(1, 2) * _I(4, 5) + _T(2, 2) * _I(5, 5))
      * _T(2, 2);

  ret.triangularView<Eigen::StrictlyLower>() = ret.transpose();

  return ret;
}

Eigen::Matrix3d parallelAxisTheorem(const Eigen::Matrix3d& _original,
                                    const Eigen::Vector3d& _comShift,
                                    double _mass)
{
  const Eigen::Vector3d& p = _comShift;
  Eigen::Matrix3d result(_original);
  for(std::size_t i=0; i<3; ++i)
    for(std::size_t j=0; j<3; ++j)
      result(i,j) += _mass * ( delta(i,j)*p.dot(p) - p(i)*p(j) );

  return result;
}

bool verifyRotation(const Eigen::Matrix3d& _T) {
  return !isNan(_T)
      && std::abs(_T.determinant() - 1.0) <= DART_EPSILON;
}

bool verifyTransform(const Eigen::Isometry3d& _T) {
  return !isNan(_T.matrix().topRows<3>())
      && std::abs(_T.linear().determinant() - 1.0) <= DART_EPSILON;
}

Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m) {
#ifndef NDEBUG
  if (std::abs(_m(0, 0)) > DART_EPSILON
      || std::abs(_m(1, 1)) > DART_EPSILON
      || std::abs(_m(2, 2)) > DART_EPSILON) {
    dtwarn << "[math::fromSkewSymmetric] Not skew a symmetric matrix:\n"
           << _m << "\n";
    return Eigen::Vector3d::Zero();
  }
#endif
  Eigen::Vector3d ret;
  ret << _m(2, 1), _m(0, 2), _m(1, 0);
  return ret;
}

Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d& _v) {
  Eigen::Matrix3d result = Eigen::Matrix3d::Zero();

  result(0, 1) = -_v(2);
  result(1, 0) =  _v(2);
  result(0, 2) =  _v(1);
  result(2, 0) = -_v(1);
  result(1, 2) = -_v(0);
  result(2, 1) =  _v(0);

  return result;
}

//==============================================================================
Eigen::Matrix3d computeRotation(const Eigen::Vector3d& axis,
                                      const AxisType axisType)
{
  assert(axis != Eigen::Vector3d::Zero());

  // First axis
  const Eigen::Vector3d axis0 = axis.normalized();

  // Second axis
  Eigen::Vector3d axis1 = axis0.cross(Eigen::Vector3d::UnitX());
  if (axis1.norm() < DART_EPSILON)
    axis1 = axis0.cross(Eigen::Vector3d::UnitY());
  axis1.normalize();

  // Third axis
  const Eigen::Vector3d axis2 = axis0.cross(axis1).normalized();

  // Assign the three axes
  Eigen::Matrix3d result;
  int index = axisType;
  result.col(index)       = axis0;
  result.col(++index % 3) = axis1;
  result.col(++index % 3) = axis2;

  assert(verifyRotation(result));

  return result;
}

//==============================================================================
Eigen::Isometry3d computeTransform(const Eigen::Vector3d& axis,
                                   const Eigen::Vector3d& translation,
                                   AxisType axisType)
{
  Eigen::Isometry3d result = Eigen::Isometry3d::Identity();

  result.linear()      = computeRotation(axis, axisType);
  result.translation() = translation;

  // Verification
  assert(verifyTransform(result));

  return result;
}

//==============================================================================
Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin,
                                      const Eigen::Vector3d& _axisZ)
{
  return computeTransform(_axisZ, _origin, AxisType::AXIS_Z);
}

//==============================================================================
SupportPolygon computeSupportPolgyon(const SupportGeometry& _geometry,
                                     const Eigen::Vector3d& _axis1,
                                     const Eigen::Vector3d& _axis2)
{
  std::vector<std::size_t> indices;
  indices.reserve(_geometry.size());
  return computeSupportPolgyon(indices, _geometry, _axis1, _axis2);
}

//==============================================================================
SupportPolygon computeSupportPolgyon(std::vector<std::size_t>& _originalIndices,
                                     const SupportGeometry& _geometry,
                                     const Eigen::Vector3d& _axis1,
                                     const Eigen::Vector3d& _axis2)
{
  SupportPolygon polygon;
  polygon.reserve(_geometry.size());
  for(const Eigen::Vector3d& v : _geometry)
    polygon.push_back(Eigen::Vector2d(v.dot(_axis1), v.dot(_axis2)));

  return computeConvexHull(_originalIndices, polygon);
}

//==============================================================================
// HullAngle is an internal struct used to facilitate the computation of 2D
// convex hulls
struct HullAngle
{
  HullAngle(double angle, double distance, std::size_t index)
    : mAngle(angle),
      mDistance(distance),
      mIndex(index)
  {
    // Do nothing
  }

  double mAngle;
  double mDistance;
  std::size_t mIndex;
};

//==============================================================================
// Comparison function to allow hull angles to be sorted
static bool HullAngleComparison(const HullAngle& a, const HullAngle& b)
{
  return a.mAngle < b.mAngle;
}

//==============================================================================
SupportPolygon computeConvexHull(const SupportPolygon& _points)
{
  std::vector<std::size_t> indices;
  indices.reserve(_points.size());
  return computeConvexHull(indices, _points);
}

//==============================================================================
Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull)
{
  if(_convexHull.size() == 0)
  {
    Eigen::Vector2d invalid = Eigen::Vector2d::Constant(std::nan(""));
    dtwarn << "[computeCentroidOfHull] Requesting the centroid of an empty set "
           << "of points! We will return <" << invalid.transpose() << ">.\n";
    return invalid;
  }

  if(_convexHull.size() == 1)
    return _convexHull[0];

  if(_convexHull.size() == 2)
    return (_convexHull[0]+_convexHull[1])/2.0;

  Eigen::Vector2d c(0,0);
  Eigen::Vector2d intersect;
  double area = 0;
  double area_i;
  Eigen::Vector2d midp12, midp01;

  for(std::size_t i=2; i < _convexHull.size(); ++i)
  {
    const Eigen::Vector2d& p0 = _convexHull[0];
    const Eigen::Vector2d& p1 = _convexHull[i-1];
    const Eigen::Vector2d& p2 = _convexHull[i];

    area_i = 0.5*( (p1[0]-p0[0])*(p2[1]-p0[1]) - (p1[1]-p0[1])*(p2[0]-p0[0]) );

    midp12 = 0.5 * (p1+p2);
    midp01 = 0.5 * (p0+p1);

    IntersectionResult result =
        computeIntersection(intersect, p0, midp12, p2, midp01);

    if(BEYOND_ENDPOINTS == result)
    {
      double a1 = atan2( (p1-p0)[1], (p1-p0)[0] )*180.0/constantsd::pi();
      double a2 = atan2( (p2-p0)[1], (p2-p0)[0] )*180.0/constantsd::pi();
      double diff = a1-a2;
      dtwarn << "[computeCentroidOfHull] You have passed in a set of points "
             << "which is not a proper convex hull! The invalid segment "
             << "contains indices " << i-1 << " -> " << i << ":\n"
             << i-1 << ") " << p1.transpose() << " (" << a1 << " degrees)" << "\n"
             << i   << ") " << p2.transpose() << " (" << a2 << " degrees)" << "\n"
             << "0) " << p0.transpose() << "\n"
             << "(" << result << ") "
             << (PARALLEL==result? "These segments are parallel!\n" :
                                   "These segments are too short to intersect!\n")
             << "Difference in angle: " << diff << "\n\n";
      continue;
    }

    area += area_i;
    c += area_i*intersect;
  }

  // A negative area means we have a bug in the code
  assert(area >= 0.0);

  if(area == 0.0)
    return c;

  return c/area;
}

//==============================================================================
// Returns true if the path that goes from p1 -> p2 -> p3 turns left
static bool isLeftTurn(const Eigen::Vector2d& p1,
                       const Eigen::Vector2d& p2,
                       const Eigen::Vector2d& p3)
{
  return (cross(p2-p1, p3-p1) > 0);
}

//==============================================================================
SupportPolygon computeConvexHull(std::vector<std::size_t>& _originalIndices,
                                 const SupportPolygon& _points)
{
  _originalIndices.clear();

  if(_points.size() <= 3)
  {
    // Three or fewer points is already a convex hull
    for(std::size_t i=0; i < _points.size(); ++i)
      _originalIndices.push_back(i);

    return _points;
  }

  // We'll use "Graham scan" to compute the convex hull in the general case
  std::size_t lowestIndex = static_cast<std::size_t>(-1);
  double lowestY = std::numeric_limits<double>::infinity();
  for(std::size_t i=0; i < _points.size(); ++i)
  {
    if(_points[i][1] < lowestY)
    {
      lowestIndex = i;
      lowestY = _points[i][1];
    }
    else if(_points[i][1] == lowestY)
    {
      if(_points[i][0] < _points[lowestIndex][0])
      {
        lowestIndex = i;
      }
    }
  }

  std::vector<HullAngle> angles;
  const Eigen::Vector2d& bottom = _points[lowestIndex];
  for(std::size_t i=0; i < _points.size(); ++i)
  {
    const Eigen::Vector2d& p = _points[i];
    if( p != bottom )
    {
      const Eigen::Vector2d& v = p - bottom;
      angles.push_back(HullAngle(atan2(v[1], v[0]), v.norm(), i));
    }
  }

  std::sort(angles.begin(), angles.end(), HullAngleComparison);

  if(angles.size() > 1)
  {
    for(std::size_t i=0; i<angles.size()-1; ++i)
    {
      if( std::abs(angles[i].mAngle - angles[i+1].mAngle) < 1e-12 )
      {
        // If two points have the same angle, throw out the one that is closer
        // to the corner
        std::size_t tossout = (angles[i].mDistance < angles[i+1].mDistance)? i : i+1;
        angles.erase(angles.begin()+tossout);
        --i;
      }
    }
  }

  if(angles.size() <= 3)
  {
    // There were so many repeated points in the given set that we only have
    // three or fewer unique points
    _originalIndices.reserve(angles.size()+1);
    _originalIndices.push_back(lowestIndex);
    for(std::size_t i=0; i < angles.size(); ++i)
      _originalIndices.push_back(angles[i].mIndex);

    SupportPolygon polygon;
    polygon.reserve(_originalIndices.size());
    for(std::size_t index : _originalIndices)
      polygon.push_back(_points[index]);

    return polygon;
  }

  std::vector<std::size_t>& edge = _originalIndices;
  std::size_t lastIndex = lowestIndex;
  std::size_t secondToLastIndex = angles[0].mIndex;
  edge.reserve(angles.size()+1);

  edge.push_back(lowestIndex);

  for(std::size_t i=1; i < angles.size(); ++i)
  {
    std::size_t currentIndex = angles[i].mIndex;
    const Eigen::Vector2d& p1 = _points[lastIndex];
    const Eigen::Vector2d& p2 = _points[secondToLastIndex];
    const Eigen::Vector2d& p3 = _points[currentIndex];

    bool leftTurn = isLeftTurn(p1, p2, p3);

    if(leftTurn)
    {
      edge.push_back(secondToLastIndex);
      lastIndex = secondToLastIndex;
      secondToLastIndex = currentIndex;
    }
    else
    {
      secondToLastIndex = edge.back();
      edge.pop_back();
      lastIndex = edge.back();
      --i;
    }
  }

  const Eigen::Vector2d& p1 = _points[edge.back()];
  const Eigen::Vector2d& p2 = _points[angles.back().mIndex];
  const Eigen::Vector2d& p3 = _points[lowestIndex];
  if(isLeftTurn(p1, p2, p3))
    edge.push_back(angles.back().mIndex);

  SupportPolygon polygon;
  polygon.reserve(edge.size());
  for(std::size_t index : edge)
    polygon.push_back(_points[index]);

  // Note that we do not need to fill in _originalIndices, because "edge" is a
  // non-const reference to _originalIndices and it has been filled in with the
  // appropriate values.
  return polygon;
}

//==============================================================================
IntersectionResult computeIntersection(Eigen::Vector2d& _intersectionPoint,
                                   const Eigen::Vector2d& a1,
                                   const Eigen::Vector2d& a2,
                                   const Eigen::Vector2d& b1,
                                   const Eigen::Vector2d& b2)
{
  double dx_a = a2[0] - a1[0];
  double dy_a = a2[1] - a1[1];

  double dx_b = b2[0] - b1[0];
  double dy_b = b2[1] - b1[1];

  Eigen::Vector2d& point = _intersectionPoint;

  if( std::abs(dx_b*dy_a - dx_a*dy_b) < 1e-12 )
  {
    // The line segments are parallel, so give back an average of all the points
    point = (a1+a2+b1+b2)/4.0;
    return PARALLEL;
  }

  point[0] = (dx_b*dy_a*a1[0] - dx_a*dy_b*b1[0] + dx_a*dx_b*(b1[1]-a1[1]))
              /(dx_b*dy_a - dx_a*dy_b);

  if( dx_a != 0.0 )
    point[1] = dy_a/dx_a*(point[0]-a1[0]) + a1[1];
  else
    point[1] = dy_b/dx_b*(point[0]-b1[0]) + b1[1];

  for(std::size_t i=0; i < 2; ++i)
  {
    if( (point[i] < std::min(a1[i], a2[i]))
        || (std::max(a1[i], a2[i]) < point[i]) )
    {
      return BEYOND_ENDPOINTS;
    }

    if( (point[i] < std::min(b1[i], b2[i]))
        || (std::max(b1[i], b2[i]) < point[i]))
    {
      return BEYOND_ENDPOINTS;
    }
  }

  return INTERSECTING;
}

//==============================================================================
double cross(const Eigen::Vector2d& _v1, const Eigen::Vector2d& _v2)
{
  return _v1[0]*_v2[1] - _v1[1]*_v2[0];
}

//==============================================================================
bool isInsideSupportPolygon(const Eigen::Vector2d& _p,
                            const SupportPolygon& _support,
                            bool _includeEdge)
{
  if(_support.size() == 0)
    return false;

  if(_support.size() == 1)
  {
    if(!_includeEdge)
      return false;

    return (_support[0] == _p);
  }

  if(_support.size() == 2)
  {
    if(!_includeEdge)
      return false;

    const Eigen::Vector2d& p1 = _support[0];
    const Eigen::Vector2d& p2 = _support[1];
    const Eigen::Vector2d& p3 = _p;

    if(cross(p2-p1, p3-p1) == 0)
    {
      if( p3[0] < std::min(p1[0], p2[0]) || std::max(p1[0], p2[0]) < p3[0] )
        return false;

      return true;
    }

    return false;
  }

  for(std::size_t i=0; i < _support.size(); ++i)
  {
    const Eigen::Vector2d& p1 = (i==0)? _support.back() : _support[i-1];
    const Eigen::Vector2d& p2 = _support[i];
    const Eigen::Vector2d& p3 = _p;

    double crossProduct = cross(p2-p1, p3-p1);
    if(crossProduct > 0.0)
      continue;

    if(crossProduct == 0)
    {
      if(!_includeEdge)
        return false;

      if( p3[0] < std::min(p1[0], p2[0]) || std::max(p1[0], p2[0]) < p3[0] )
        return false;

      return true;
    }
    else
    {
      return false;
    }
  }

  return true;
}

//==============================================================================
Eigen::Vector2d computeClosestPointOnLineSegment(const Eigen::Vector2d& _p,
                                          const Eigen::Vector2d& _s1,
                                          const Eigen::Vector2d& _s2)
{
  Eigen::Vector2d result;

  if(_s1[0] - _s2[0] == 0)
  {
    result[0] = _s1[0];
    result[1] = _p[1];

    if( result[1] < std::min(_s1[1], _s2[1])
        || std::max(_s1[1], _s2[1]) < result[1] )
    {
      if( std::abs(_p[1]-_s2[1]) < std::abs(_p[1]-_s1[1]) )
        result[1] = _s2[1];
      else
        result[1] = _s1[1];
    }
  }
  else
  {
    double m = (_s2[1]-_s1[1]) / (_s2[0]-_s1[0]);
    double k = _s1[1] - m*_s1[0];
    result[0] = (_p[0] + m*(_p[1] - k))/(m*m+1.0);
    result[1] = m*result[0] + k;

    if( result[0] < std::min(_s1[0], _s2[0])
        || std::max(_s1[0], _s2[0]) < result[0] )
    {
      if( (_p - _s2).norm() < (_p - _s1).norm() )
        result = _s2;
      else
        result = _s1;
    }
  }

  return result;
}

//==============================================================================
Eigen::Vector2d computeClosestPointOnSupportPolygon(const Eigen::Vector2d& _p,
                                             const SupportPolygon& _support)
{
  std::size_t _index1;
  std::size_t _index2;
  return computeClosestPointOnSupportPolygon(_index1, _index2, _p, _support);
}

//==============================================================================
Eigen::Vector2d computeClosestPointOnSupportPolygon(std::size_t& _index1, std::size_t& _index2,
                                             const Eigen::Vector2d& _p,
                                             const SupportPolygon& _support)
{
  if(_support.size() == 0)
  {
    _index1 = static_cast<std::size_t>(-1);
    _index2 = _index1;
    return _p;
  }

  if(_support.size() == 1)
  {
    _index1 = 0;
    _index2 = 0;
    return _support[0];
  }

  if(_support.size() == 2)
  {
    _index1 = 0;
    _index2 = 1;
    return computeClosestPointOnLineSegment(_p, _support[0], _support[1]);
  }

  double best = std::numeric_limits<double>::infinity(), check;
  Eigen::Vector2d test, result;
  for(std::size_t i=0; i < _support.size(); ++i)
  {
    const Eigen::Vector2d& p1 = (i==0)? _support.back() : _support[i-1];
    const Eigen::Vector2d& p2 = _support[i];

    test = computeClosestPointOnLineSegment(_p, p1, p2);
    check = (test - _p).norm();
    if(check < best)
    {
      best = check;
      result = test;
      _index1 = (i==0)? _support.size()-1 : i-1;
      _index2 = i;
    }
  }

  return result;
}

BoundingBox::BoundingBox() :
        mMin(0, 0, 0), mMax(0, 0, 0)
{

}
BoundingBox::BoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max) :
        mMin(min), mMax(max)
{

}


}  // namespace math
}  // namespace dart
