minkowski_diff-inl.h

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2016, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  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.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  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 OWNER 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 FCL_NARROWPHASE_DETAIL_MINKOWSKIDIFF_INL_H
#define FCL_NARROWPHASE_DETAIL_MINKOWSKIDIFF_INL_H

#include "fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h"

#include "fcl/geometry/shape/box.h"
#include "fcl/geometry/shape/capsule.h"
#include "fcl/geometry/shape/cone.h"
#include "fcl/geometry/shape/convex.h"
#include "fcl/geometry/shape/cylinder.h"
#include "fcl/geometry/shape/ellipsoid.h"
#include "fcl/geometry/shape/halfspace.h"
#include "fcl/geometry/shape/plane.h"
#include "fcl/geometry/shape/sphere.h"
#include "fcl/geometry/shape/triangle_p.h"

namespace fcl
{

namespace detail
{

//==============================================================================
extern template
struct MinkowskiDiff<double>;

//==============================================================================
template <typename S, typename Derived>
Vector3<S> getSupport(
    const ShapeBase<S>* shape,
    const Eigen::MatrixBase<Derived>& dir)
{
  // Check the number of rows is 6 at compile time
  EIGEN_STATIC_ASSERT(
        Derived::RowsAtCompileTime == 3
        && Derived::ColsAtCompileTime == 1,
        THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);

  switch(shape->getNodeType())
  {
  case GEOM_TRIANGLE:
    {
      const auto* triangle = static_cast<const TriangleP<S>*>(shape);
      S dota = dir.dot(triangle->a);
      S dotb = dir.dot(triangle->b);
      S dotc = dir.dot(triangle->c);
      if(dota > dotb)
      {
        if(dotc > dota)
          return triangle->c;
        else
          return triangle->a;
      }
      else
      {
        if(dotc > dotb)
          return triangle->c;
        else
          return triangle->b;
      }
    }
    break;
  case GEOM_BOX:
    {
      const Box<S>* box = static_cast<const Box<S>*>(shape);
      return Vector3<S>((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2),
                   (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2),
                   (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2));
    }
    break;
  case GEOM_SPHERE:
    {
      const Sphere<S>* sphere = static_cast<const Sphere<S>*>(shape);
      return dir * sphere->radius;
    }
    break;
  case GEOM_ELLIPSOID:
    {
      const Ellipsoid<S>* ellipsoid = static_cast<const Ellipsoid<S>*>(shape);

      const S a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
      const S b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
      const S c2 = ellipsoid->radii[2] * ellipsoid->radii[2];

      const Vector3<S> v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
      const S d = std::sqrt(v.dot(dir));

      return v / d;
    }
    break;
  case GEOM_CAPSULE:
    {
      const Capsule<S>* capsule = static_cast<const Capsule<S>*>(shape);
      S half_h = capsule->lz * 0.5;
      Vector3<S> pos1(0, 0, half_h);
      Vector3<S> pos2(0, 0, -half_h);
      Vector3<S> v = dir * capsule->radius;
      pos1 += v;
      pos2 += v;
      if(dir.dot(pos1) > dir.dot(pos2))
        return pos1;
      else return pos2;
    }
    break;
  case GEOM_CONE:
    {
      const Cone<S>* cone = static_cast<const Cone<S>*>(shape);
      S zdist = dir[0] * dir[0] + dir[1] * dir[1];
      S len = zdist + dir[2] * dir[2];
      zdist = std::sqrt(zdist);
      len = std::sqrt(len);
      S half_h = cone->lz * 0.5;
      S radius = cone->radius;

      S sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);

      if(dir[2] > len * sin_a)
        return Vector3<S>(0, 0, half_h);
      else if(zdist > 0)
      {
        S rad = radius / zdist;
        return Vector3<S>(rad * dir[0], rad * dir[1], -half_h);
      }
      else
        return Vector3<S>(0, 0, -half_h);
    }
    break;
  case GEOM_CYLINDER:
    {
      const Cylinder<S>* cylinder = static_cast<const Cylinder<S>*>(shape);
      S zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
      S half_h = cylinder->lz * 0.5;
      if(zdist == 0.0)
      {
        return Vector3<S>(0, 0, (dir[2]>0)? half_h:-half_h);
      }
      else
      {
        S d = cylinder->radius / zdist;
        return Vector3<S>(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h);
      }
    }
    break;
  case GEOM_CONVEX:
    {
      const Convex<S>* convex = static_cast<const Convex<S>*>(shape);
      S maxdot = - std::numeric_limits<S>::max();
      Vector3<S>* curp = convex->points;
      Vector3<S> bestv = Vector3<S>::Zero();
      for(int i = 0; i < convex->num_points; ++i, curp+=1)
      {
        S dot = dir.dot(*curp);
        if(dot > maxdot)
        {
          bestv = *curp;
          maxdot = dot;
        }
      }
      return bestv;
    }
    break;
  case GEOM_PLANE:
  break;
  default:
    ; // nothing
  }

  return Vector3<S>::Zero();
}

//==============================================================================
template <typename S>
MinkowskiDiff<S>::MinkowskiDiff()
{
  // Do nothing
}

//==============================================================================
template <typename S>
Vector3<S> MinkowskiDiff<S>::support0(const Vector3<S>& d) const
{
  return getSupport(shapes[0], d);
}

//==============================================================================
template <typename S>
Vector3<S> MinkowskiDiff<S>::support1(const Vector3<S>& d) const
{
  return toshape0 * getSupport(shapes[1], toshape1 * d);
}

//==============================================================================
template <typename S>
Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d) const
{
  return support0(d) - support1(-d);
}

//==============================================================================
template <typename S>
Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d, size_t index) const
{
  if(index)
    return support1(d);
  else
    return support0(d);
}

//==============================================================================
template <typename S>
Vector3<S> MinkowskiDiff<S>::support0(const Vector3<S>& d, const Vector3<S>& v) const
{
  if(d.dot(v) <= 0)
    return getSupport(shapes[0], d);
  else
    return getSupport(shapes[0], d) + v;
}

//==============================================================================
template <typename S>
Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d, const Vector3<S>& v) const
{
  return support0(d, v) - support1(-d);
}

//==============================================================================
template <typename S>
Vector3<S> MinkowskiDiff<S>::support(const Vector3<S>& d, const Vector3<S>& v, size_t index) const
{
  if(index)
    return support1(d);
  else
    return support0(d, v);
}

} // namespace detail
} // namespace fcl

#endif