narrowphase.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
 *  All rights reserved.
 *  Copyright (c) 2021-2022, INRIA
 *  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 HPP_FCL_NARROWPHASE_H
#define HPP_FCL_NARROWPHASE_H

#include <limits>
#include <iostream>

#include <hpp/fcl/narrowphase/gjk.h>
#include <hpp/fcl/collision_data.h>

namespace hpp {
namespace fcl {

struct HPP_FCL_DLLAPI GJKSolver {
  typedef Eigen::Array<FCL_REAL, 1, 2> Array2d;

  template <typename S1, typename S2>
  void initialize_gjk(details::GJK& gjk, const details::MinkowskiDiff& shape,
                      const S1& s1, const S2& s2, Vec3f& guess,
                      support_func_guess_t& support_hint) const {
    switch (gjk_initial_guess) {
      case GJKInitialGuess::DefaultGuess:
        guess = Vec3f(1, 0, 0);
        support_hint.setZero();
        break;
      case GJKInitialGuess::CachedGuess:
        guess = cached_guess;
        support_hint = support_func_cached_guess;
        break;
      case GJKInitialGuess::BoundingVolumeGuess:
        if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
          HPP_FCL_THROW_PRETTY(
              "computeLocalAABB must have been called on the shapes before "
              "using "
              "GJKInitialGuess::BoundingVolumeGuess.",
              std::logic_error);
        }
        guess.noalias() = s1.aabb_local.center() -
                          (shape.oR1 * s2.aabb_local.center() + shape.ot1);
        support_hint.setZero();
        break;
      default:
        HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
    }
    // TODO: use gjk_initial_guess instead
    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    if (enable_cached_guess) {
      guess = cached_guess;
      support_hint = support_func_cached_guess;
    }
    HPP_FCL_COMPILER_DIAGNOSTIC_POP

    gjk.setDistanceEarlyBreak(distance_upper_bound);

    gjk.gjk_variant = gjk_variant;
    gjk.convergence_criterion = gjk_convergence_criterion;
    gjk.convergence_criterion_type = gjk_convergence_criterion_type;
  }

  template <typename S1, typename S2>
  bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2,
                      const Transform3f& tf2, FCL_REAL& distance_lower_bound,
                      bool enable_penetration, Vec3f* contact_points,
                      Vec3f* normal) const {
    details::MinkowskiDiff shape;
    shape.set(&s1, &s2, tf1, tf2);

    Vec3f guess;
    support_func_guess_t support_hint;
    details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
    initialize_gjk(gjk, shape, s1, s2, guess, support_hint);

    details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
        enable_cached_guess) {
      cached_guess = gjk.getGuessFromSimplex();
      support_func_cached_guess = gjk.support_hint;
    }
    HPP_FCL_COMPILER_DIAGNOSTIC_POP

    Vec3f w0, w1;
    switch (gjk_status) {
      case details::GJK::Inside:
        if (!enable_penetration && contact_points == NULL && normal == NULL)
          return true;
        if (gjk.hasPenetrationInformation(shape)) {
          gjk.getClosestPoints(shape, w0, w1);
          distance_lower_bound = gjk.distance;
          if (normal)
            (*normal).noalias() = tf1.getRotation() * (w0 - w1).normalized();
          if (contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
          return true;
        } else {
          details::EPA epa(epa_max_face_num, epa_max_vertex_num,
                           epa_max_iterations, epa_tolerance);
          details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
          if (epa_status & details::EPA::Valid ||
              epa_status == details::EPA::OutOfFaces        // Warnings
              || epa_status == details::EPA::OutOfVertices  // Warnings
          ) {
            epa.getClosestPoints(shape, w0, w1);
            distance_lower_bound = -epa.depth;
            if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal;
            if (contact_points)
              *contact_points =
                  tf1.transform(w0 - epa.normal * (epa.depth * 0.5));
            return true;
          } else if (epa_status == details::EPA::FallBack) {
            epa.getClosestPoints(shape, w0, w1);
            distance_lower_bound = -epa.depth;  // Should be zero
            if (normal) (*normal).noalias() = tf1.getRotation() * epa.normal;
            if (contact_points) *contact_points = tf1.transform(w0);
            return true;
          }
          distance_lower_bound = -(std::numeric_limits<FCL_REAL>::max)();
          // EPA failed but we know there is a collision so we should
          return true;
        }
        break;
      case details::GJK::Valid:
        distance_lower_bound = gjk.distance;
        break;
      default:;
    }

    return false;
  }

  template <typename S>
  bool shapeTriangleInteraction(const S& s, const Transform3f& tf1,
                                const Vec3f& P1, const Vec3f& P2,
                                const Vec3f& P3, const Transform3f& tf2,
                                FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                                Vec3f& normal) const {
    bool col;
    // Express everything in frame 1
    const Transform3f tf_1M2(tf1.inverseTimes(tf2));
    TriangleP tri(tf_1M2.transform(P1), tf_1M2.transform(P2),
                  tf_1M2.transform(P3));

    details::MinkowskiDiff shape;
    shape.set(&s, &tri);

    Vec3f guess;
    support_func_guess_t support_hint;
    details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
    initialize_gjk(gjk, shape, s, tri, guess, support_hint);

    details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);

    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
    HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
        enable_cached_guess) {
      cached_guess = gjk.getGuessFromSimplex();
      support_func_cached_guess = gjk.support_hint;
    }
    HPP_FCL_COMPILER_DIAGNOSTIC_PUSH

    Vec3f w0, w1;
    switch (gjk_status) {
      case details::GJK::Inside:
        col = true;
        if (gjk.hasPenetrationInformation(shape)) {
          gjk.getClosestPoints(shape, w0, w1);
          distance = gjk.distance;
          normal.noalias() = tf1.getRotation() * (w0 - w1).normalized();
          p1 = p2 = tf1.transform((w0 + w1) / 2);
        } else {
          details::EPA epa(epa_max_face_num, epa_max_vertex_num,
                           epa_max_iterations, epa_tolerance);
          details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
          if (epa_status & details::EPA::Valid ||
              epa_status == details::EPA::OutOfFaces        // Warnings
              || epa_status == details::EPA::OutOfVertices  // Warnings
          ) {
            epa.getClosestPoints(shape, w0, w1);
            distance = -epa.depth;
            normal.noalias() = tf1.getRotation() * epa.normal;
            p1 = p2 = tf1.transform(w0 - epa.normal * (epa.depth * 0.5));
            assert(distance <= 1e-6);
          } else {
            distance = -(std::numeric_limits<FCL_REAL>::max)();
            gjk.getClosestPoints(shape, w0, w1);
            p1 = p2 = tf1.transform(w0);
          }
        }
        break;
      case details::GJK::Valid:
      case details::GJK::EarlyStopped:
      case details::GJK::Failed:
        col = false;

        gjk.getClosestPoints(shape, p1, p2);
        // TODO On degenerated case, the closest point may be wrong
        // (i.e. an object face normal is colinear to gjk.ray
        // assert (distance == (w0 - w1).norm());
        distance = gjk.distance;

        p1 = tf1.transform(p1);
        p2 = tf1.transform(p2);
        assert(distance > 0);
        break;
      default:
        assert(false && "should not reach type part.");
        throw std::logic_error("GJKSolver: should not reach this part.");
    }
    return col;
  }

  template <typename S1, typename S2>
  bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2,
                     const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
                     Vec3f& p2, Vec3f& normal) const {
#ifndef NDEBUG
    FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
#endif
    details::MinkowskiDiff shape;
    shape.set(&s1, &s2, tf1, tf2);

    Vec3f guess;
    support_func_guess_t support_hint;
    details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
    initialize_gjk(gjk, shape, s1, s2, guess, support_hint);

    details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
    if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
        enable_cached_guess) {
      cached_guess = gjk.getGuessFromSimplex();
      support_func_cached_guess = gjk.support_hint;
    }

    if (gjk_status == details::GJK::Failed) {
      // TODO: understand why GJK fails between cylinder and box
      assert(distance * distance < sqrt(eps));
      Vec3f w0, w1;
      gjk.getClosestPoints(shape, w0, w1);
      distance = 0;
      p1 = tf1.transform(w0);
      p2 = tf1.transform(w1);
      normal.setZero();
      return false;
    } else if (gjk_status == details::GJK::Valid) {
      gjk.getClosestPoints(shape, p1, p2);
      // TODO On degenerated case, the closest point may be wrong
      // (i.e. an object face normal is colinear to gjk.ray
      // assert (distance == (w0 - w1).norm());
      distance = gjk.distance;

      normal.noalias() = tf1.getRotation() * gjk.ray;
      normal.normalize();
      p1 = tf1.transform(p1);
      p2 = tf1.transform(p2);
      return true;
    } else if (gjk_status == details::GJK::EarlyStopped) {
      distance = gjk.distance;
      p1 = p2 = normal =
          Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN());
      return true;
    } else {
      assert(gjk_status == details::GJK::Inside);
      if (gjk.hasPenetrationInformation(shape)) {
        gjk.getClosestPoints(shape, p1, p2);
        distance = gjk.distance;
        // Return contact points in case of collision
        // p1 = tf1.transform (p1);
        // p2 = tf1.transform (p2);
        normal.noalias() = tf1.getRotation() * (p1 - p2);
        normal.normalize();
        p1 = tf1.transform(p1);
        p2 = tf1.transform(p2);
      } else {
        details::EPA epa(epa_max_face_num, epa_max_vertex_num,
                         epa_max_iterations, epa_tolerance);
        details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
        if (epa_status & details::EPA::Valid ||
            epa_status == details::EPA::OutOfFaces        // Warnings
            || epa_status == details::EPA::OutOfVertices  // Warnings
            || epa_status == details::EPA::FallBack) {
          Vec3f w0, w1;
          epa.getClosestPoints(shape, w0, w1);
          assert(epa.depth >= -eps);
          distance = (std::min)(0., -epa.depth);
          normal.noalias() = tf1.getRotation() * epa.normal;
          p1 = tf1.transform(w0);
          p2 = tf1.transform(w1);
          return false;
        }
        distance = -(std::numeric_limits<FCL_REAL>::max)();
        gjk.getClosestPoints(shape, p1, p2);
        p1 = tf1.transform(p1);
        p2 = tf1.transform(p2);
      }
      return false;
    }
  }

  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
  GJKSolver() {
    gjk_max_iterations = 128;
    gjk_tolerance = 1e-6;
    epa_max_face_num = 128;
    epa_max_vertex_num = 64;
    epa_max_iterations = 255;
    epa_tolerance = 1e-6;
    enable_cached_guess = false;  // TODO: use gjk_initial_guess instead
    cached_guess = Vec3f(1, 0, 0);
    support_func_cached_guess = support_func_guess_t::Zero();
    distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
    gjk_initial_guess = GJKInitialGuess::DefaultGuess;
    gjk_variant = GJKVariant::DefaultGJK;
    gjk_convergence_criterion = GJKConvergenceCriterion::VDB;
    gjk_convergence_criterion_type = GJKConvergenceCriterionType::Relative;
  }

  GJKSolver(const DistanceRequest& request) {
    cached_guess = Vec3f(1, 0, 0);
    support_func_cached_guess = support_func_guess_t::Zero();
    distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();

    // EPS settings
    epa_max_face_num = 128;
    epa_max_vertex_num = 64;
    epa_max_iterations = 255;
    epa_tolerance = 1e-6;

    set(request);
  }

  void set(const DistanceRequest& request) {
    gjk_initial_guess = request.gjk_initial_guess;
    // TODO: use gjk_initial_guess instead
    enable_cached_guess = request.enable_cached_gjk_guess;
    gjk_variant = request.gjk_variant;
    gjk_convergence_criterion = request.gjk_convergence_criterion;
    gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
    gjk_tolerance = request.gjk_tolerance;
    gjk_max_iterations = request.gjk_max_iterations;
    if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
        enable_cached_guess) {
      cached_guess = request.cached_gjk_guess;
      support_func_cached_guess = request.cached_support_func_guess;
    }
  }

  GJKSolver(const CollisionRequest& request) {
    cached_guess = Vec3f(1, 0, 0);
    support_func_cached_guess = support_func_guess_t::Zero();
    distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();

    // EPS settings
    epa_max_face_num = 128;
    epa_max_vertex_num = 64;
    epa_max_iterations = 255;
    epa_tolerance = 1e-6;

    set(request);
  }

  void set(const CollisionRequest& request) {
    gjk_initial_guess = request.gjk_initial_guess;
    // TODO: use gjk_initial_guess instead
    enable_cached_guess = request.enable_cached_gjk_guess;
    gjk_variant = request.gjk_variant;
    gjk_convergence_criterion = request.gjk_convergence_criterion;
    gjk_convergence_criterion_type = request.gjk_convergence_criterion_type;
    gjk_tolerance = request.gjk_tolerance;
    gjk_max_iterations = request.gjk_max_iterations;
    if (gjk_initial_guess == GJKInitialGuess::CachedGuess ||
        enable_cached_guess) {
      cached_guess = request.cached_gjk_guess;
      support_func_cached_guess = request.cached_support_func_guess;
    }

    // The distance upper bound should be at least greater to the requested
    // security margin. Otherwise, we will likely miss some collisions.
    distance_upper_bound = (std::max)(
        0., (std::max)(request.distance_upper_bound, request.security_margin));
  }

  GJKSolver(const GJKSolver& other) = default;

  HPP_FCL_COMPILER_DIAGNOSTIC_PUSH
  HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
  bool operator==(const GJKSolver& other) const {
    return epa_max_face_num == other.epa_max_face_num &&
           epa_max_vertex_num == other.epa_max_vertex_num &&
           epa_max_iterations == other.epa_max_iterations &&
           epa_tolerance == other.epa_tolerance &&
           gjk_max_iterations == other.gjk_max_iterations &&
           enable_cached_guess ==
               other.enable_cached_guess &&  // TODO: use gjk_initial_guess
                                             // instead
           cached_guess == other.cached_guess &&
           support_func_cached_guess == other.support_func_cached_guess &&
           distance_upper_bound == other.distance_upper_bound &&
           gjk_initial_guess == other.gjk_initial_guess &&
           gjk_variant == other.gjk_variant &&
           gjk_convergence_criterion == other.gjk_convergence_criterion &&
           gjk_convergence_criterion_type ==
               other.gjk_convergence_criterion_type;
  }
  HPP_FCL_COMPILER_DIAGNOSTIC_POP

  bool operator!=(const GJKSolver& other) const { return !(*this == other); }

  unsigned int epa_max_face_num;

  unsigned int epa_max_vertex_num;

  unsigned int epa_max_iterations;

  FCL_REAL epa_tolerance;

  mutable FCL_REAL gjk_tolerance;

  mutable size_t gjk_max_iterations;

  HPP_FCL_DEPRECATED_MESSAGE("Use gjk_initial_guess instead")
  mutable bool enable_cached_guess;

  mutable Vec3f cached_guess;

  mutable GJKInitialGuess gjk_initial_guess;

  mutable GJKVariant gjk_variant;

  mutable GJKConvergenceCriterion gjk_convergence_criterion;

  mutable GJKConvergenceCriterionType gjk_convergence_criterion_type;

  mutable support_func_guess_t support_func_cached_guess;

  mutable FCL_REAL distance_upper_bound;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <>
HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
    const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
    const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
    Vec3f& p2, Vec3f& normal) const;

template <>
HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
    const Halfspace& s, const Transform3f& tf1, const Vec3f& P1,
    const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
    FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const;

template <>
HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction(
    const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
    const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
    Vec3f& p2, Vec3f& normal) const;

#define SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2)                 \
  template <>                                                       \
  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<S1, S2>(            \
      const S1& s1, const Transform3f& tf1, const S2& s2,           \
      const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool, \
      Vec3f* contact_points, Vec3f* normal) const

#define SHAPE_INTERSECT_SPECIALIZATION(S1, S2) \
  SHAPE_INTERSECT_SPECIALIZATION_BASE(S1, S2); \
  SHAPE_INTERSECT_SPECIALIZATION_BASE(S2, S1)

SHAPE_INTERSECT_SPECIALIZATION(Sphere, Capsule);
SHAPE_INTERSECT_SPECIALIZATION_BASE(Sphere, Sphere);
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Box);
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Halfspace);
SHAPE_INTERSECT_SPECIALIZATION(Sphere, Plane);

SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Box);
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Capsule);
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cylinder);
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Cone);
SHAPE_INTERSECT_SPECIALIZATION(Halfspace, Plane);

SHAPE_INTERSECT_SPECIALIZATION(Plane, Box);
SHAPE_INTERSECT_SPECIALIZATION(Plane, Capsule);
SHAPE_INTERSECT_SPECIALIZATION(Plane, Cylinder);
SHAPE_INTERSECT_SPECIALIZATION(Plane, Cone);

#undef SHAPE_INTERSECT_SPECIALIZATION
#undef SHAPE_INTERSECT_SPECIALIZATION_BASE

#define SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2)                  \
  template <>                                                       \
  HPP_FCL_DLLAPI bool GJKSolver::shapeDistance<S1, S2>(             \
      const S1& s1, const Transform3f& tf1, const S2& s2,           \
      const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
      Vec3f& normal) const

#define SHAPE_DISTANCE_SPECIALIZATION(S1, S2) \
  SHAPE_DISTANCE_SPECIALIZATION_BASE(S1, S2); \
  SHAPE_DISTANCE_SPECIALIZATION_BASE(S2, S1)

SHAPE_DISTANCE_SPECIALIZATION(Sphere, Capsule);
SHAPE_DISTANCE_SPECIALIZATION(Sphere, Box);
SHAPE_DISTANCE_SPECIALIZATION(Sphere, Cylinder);
SHAPE_DISTANCE_SPECIALIZATION_BASE(Sphere, Sphere);
SHAPE_DISTANCE_SPECIALIZATION_BASE(Capsule, Capsule);
SHAPE_DISTANCE_SPECIALIZATION_BASE(TriangleP, TriangleP);

#undef SHAPE_DISTANCE_SPECIALIZATION
#undef SHAPE_DISTANCE_SPECIALIZATION_BASE

#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wc99-extensions"
#endif

// param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no dot for some obscure reasons).
#define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc)      \
  \
  doc template <>                                                 \
  HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Shape1, Shape2>(  \
      const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
      const Transform3f& tf2, FCL_REAL& distance_lower_bound,     \
      bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape, doc) \
  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape, Shape, doc)
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1, Shape2, doc) \
  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1, Shape2, doc);           \
  HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape2, Shape1, doc)

HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Sphere, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Capsule, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane, );

template <>
HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Sphere>(
    const Box& s1, const Transform3f& tf1, const Sphere& s2,
    const Transform3f& tf2, FCL_REAL& distance_lower_bound,
    bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const;

#ifdef IS_DOXYGEN  // for doxygen only

template <>
HPP_FCL_DLLAPI bool GJKSolver::shapeIntersect<Box, Box>(
    const Box& s1, const Transform3f& tf1, const Box& s2,
    const Transform3f& tf2, FCL_REAL& distance_lower_bound,
    bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const;
#endif
// HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane, );

HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Halfspace, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Capsule, Plane, );

HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Halfspace, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cylinder, Plane, );

HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Halfspace, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Cone, Plane, );

HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Halfspace, );

HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Plane, );
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Plane, Halfspace, );

#undef HPP_FCL_DECLARE_SHAPE_INTERSECT
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF
#undef HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR



// param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no dot for some obscure reasons).
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape, doc)                  \
 \
  doc template <>                                                   \
  HPP_FCL_DLLAPI bool GJKSolver::shapeTriangleInteraction<Shape>(   \
      const Shape& s, const Transform3f& tf1, const Vec3f& P1,      \
      const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,     \
      FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const

HPP_FCL_DECLARE_SHAPE_TRIANGLE(Sphere, );
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Halfspace, );
HPP_FCL_DECLARE_SHAPE_TRIANGLE(Plane, );

#undef HPP_FCL_DECLARE_SHAPE_TRIANGLE



// param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no dot for some obscure reasons).
#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc)         \
     \
  doc template <>                                                   \
  bool HPP_FCL_DLLAPI GJKSolver::shapeDistance<Shape1, Shape2>(     \
      const Shape1& s1, const Transform3f& tf1, const Shape2& s2,   \
      const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2, \
      Vec3f& normal) const
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Shape, doc) \
  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape, Shape, doc)
#define HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Shape1, Shape2, doc) \
  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1, Shape2, doc);           \
  HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape2, Shape1, doc)

HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Box, );
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Capsule, );
HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR(Sphere, Cylinder, );
HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(Sphere, );

HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(
    Capsule,
);

HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF(
    TriangleP,
);

#undef HPP_FCL_DECLARE_SHAPE_DISTANCE
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_SELF
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR

#if !(__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
#pragma GCC diagnostic pop
#endif
}  // namespace fcl

}  // namespace hpp

#endif