Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/narrowphase/narrowphase.cpp	Lines:	51	160	31.9 %
Date:	2024-02-09 12:57:42	Branches:	42	292	14.4 %


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, 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.
 */

/** \author Jia Pan */

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

#include <vector>

#include <hpp/fcl/shape/geometric_shapes_utility.h>
#include <hpp/fcl/internal/intersect.h>
#include "details.h"

namespace hpp {
namespace fcl {
// Shape intersect algorithms based on:
// - built-in function: 0
// - GJK:               1
//
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | capsule | cone | cylinder | plane | half-space
// | triangle |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | box        |  0  |   0    |    1    |   1  |    1     |   0   |      0 | 1
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   0    |    0    |   1  |    1     |   0   |      0 | 0
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|    1    |   1  |    1     |   0   |      0 | 1
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|/////////|   1  |    1     |   0   |      0 | 1
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|/////////|//////|    1     |   0   |      0 | 1
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|/////////|//////|//////////|   0   |      0 | 0
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|/////////|//////|//////////|///////|      0 | 0
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | triangle |/////|////////|/////////|//////|//////////|///////|////////////|
// 1     |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+

#define SHAPE_INTERSECT_INVERTED(Shape1, Shape2)                             \
  template <>                                                                \
  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 { \
    bool res = shapeIntersect(s2, tf2, s1, tf1, distance_lower_bound,        \
                              enable_penetration, contact_points, normal);   \
    (*normal) *= -1.0;                                                       \
    return res;                                                              \
  }

template <>
bool GJKSolver::shapeIntersect<Sphere, Capsule>(
    const Sphere& s1, const Transform3f& tf1, const Capsule& s2,
    const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool,
    Vec3f* contact_points, Vec3f* normal) const {
  return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, distance_lower_bound,
                                         contact_points, normal);
}

SHAPE_INTERSECT_INVERTED(Capsule, Sphere)

template <>
bool GJKSolver::shapeIntersect<Sphere, Sphere>(
    const Sphere& s1, const Transform3f& tf1, const Sphere& s2,
    const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool,
    Vec3f* contact_points, Vec3f* normal) const {
  return details::sphereSphereIntersect(s1, tf1, s2, tf2, distance_lower_bound,
                                        contact_points, normal);
}

template <>
bool GJKSolver::shapeIntersect<Box, Sphere>(
    const Box& s1, const Transform3f& tf1, const Sphere& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f ps, pb, n;
  bool res = details::boxSphereDistance(s1, tf1, s2, tf2, distance, ps, pb, n);
  if (normal) *normal = n;
  if (contact_points) *contact_points = pb;
  return res;
}

SHAPE_INTERSECT_INVERTED(Sphere, Box)

/*
template<>
bool GJKSolver::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
                                         const Box& s2, const Transform3f& tf2,
                                         FCL_REAL& distance_lower_bound,
                                         bool,
                                         Vec3f* contact_points, Vec3f* normal)
const
{
  return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points,
&distance_lower_bound, normal);
}
*/

template <>
bool GJKSolver::shapeIntersect<Sphere, Halfspace>(
    const Sphere& s1, const Transform3f& tf1, const Halfspace& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res =
      details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Halfspace, Sphere)

template <>
bool GJKSolver::shapeIntersect<Box, Halfspace>(
    const Box& s1, const Transform3f& tf1, const Halfspace& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res =
      details::boxHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Halfspace, Box)

template <>
bool GJKSolver::shapeIntersect<Capsule, Halfspace>(
    const Capsule& s1, const Transform3f& tf1, const Halfspace& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res =
      details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Halfspace, Capsule)

template <>
bool GJKSolver::shapeIntersect<Cylinder, Halfspace>(
    const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res = details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
                                                 p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Halfspace, Cylinder)

template <>
bool GJKSolver::shapeIntersect<Cone, Halfspace>(
    const Cone& s1, const Transform3f& tf1, const Halfspace& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res =
      details::coneHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Halfspace, Cone)

template <>
bool GJKSolver::shapeIntersect<Halfspace, Halfspace>(
    const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /*contact_points*/,
    Vec3f* /*normal*/) const {
  Halfspace s;
  Vec3f p, d;
  FCL_REAL depth;
  int ret;
  bool res = details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
  distance = -depth;
  return res;
}

template <>
bool GJKSolver::shapeIntersect<Plane, Halfspace>(
    const Plane& s1, const Transform3f& tf1, const Halfspace& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /*contact_points*/,
    Vec3f* /*normal*/) const {
  Plane pl;
  Vec3f p, d;
  FCL_REAL depth;
  int ret;
  bool res =
      details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
  distance = -depth;
  return res;
}

SHAPE_INTERSECT_INVERTED(Halfspace, Plane)

template <>
bool GJKSolver::shapeIntersect<Sphere, Plane>(
    const Sphere& s1, const Transform3f& tf1, const Plane& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res =
      details::spherePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Plane, Sphere)

template <>
bool GJKSolver::shapeIntersect<Box, Plane>(
    const Box& s1, const Transform3f& tf1, const Plane& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res = details::boxPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Plane, Box)

template <>
bool GJKSolver::shapeIntersect<Capsule, Plane>(
    const Capsule& s1, const Transform3f& tf1, const Plane& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res =
      details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Plane, Capsule)

template <>
bool GJKSolver::shapeIntersect<Cylinder, Plane>(
    const Cylinder& s1, const Transform3f& tf1, const Plane& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res =
      details::cylinderPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Plane, Cylinder)

template <>
bool GJKSolver::shapeIntersect<Cone, Plane>(
    const Cone& s1, const Transform3f& tf1, const Plane& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  Vec3f p1, p2, n;
  bool res = details::conePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
  if (contact_points) *contact_points = p1;
  if (normal) *normal = n;
  return res;
}

SHAPE_INTERSECT_INVERTED(Plane, Cone)

template <>
bool GJKSolver::shapeIntersect<Plane, Plane>(
    const Plane& s1, const Transform3f& tf1, const Plane& s2,
    const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
    Vec3f* normal) const {
  return details::planeIntersect(s1, tf1, s2, tf2, contact_points, &distance,
                                 normal);
}

template <>
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 {
  return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1),
                                          tf2.transform(P2), tf2.transform(P3),
                                          distance, p1, p2, normal);
}

template <>
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 {
  return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance,
                                             p1, p2, normal);
}

template <>
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 {
  return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance, p1,
                                         p2, normal);
}

// Shape distance algorithms not using built-in GJK algorithm
//
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | capsule | cone | cylinder | plane | half-space
// | triangle |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | box        |     |   O    |         |      |          |       | | |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   O    |    O    |      |    O     |       | | |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|    O    |      |          |       | | |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|/////////|      |          |       | | |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|/////////|//////|          |       | | |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|/////////|//////|//////////|       | | |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|/////////|//////|//////////|///////| | |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
// | triangle |/////|////////|/////////|//////|//////////|///////|////////////|
// |
// +------------+-----+--------+---------+------+----------+-------+------------+----------+

template <>
bool GJKSolver::shapeDistance<Sphere, Capsule>(const Sphere& s1,
                                               const Transform3f& tf1,
                                               const Capsule& s2,
                                               const Transform3f& tf2,
                                               FCL_REAL& dist, Vec3f& p1,
                                               Vec3f& p2, Vec3f& normal) const {
  return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
}

template <>
bool GJKSolver::shapeDistance<Capsule, Sphere>(const Capsule& s1,
                                               const Transform3f& tf1,
                                               const Sphere& s2,
                                               const Transform3f& tf2,
                                               FCL_REAL& dist, Vec3f& p1,
                                               Vec3f& p2, Vec3f& normal) const {
  return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
}

template <>
bool GJKSolver::shapeDistance<Box, Sphere>(const Box& s1,
                                           const Transform3f& tf1,
                                           const Sphere& s2,
                                           const Transform3f& tf2,
                                           FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
                                           Vec3f& normal) const {
  return !details::boxSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
}

template <>
bool GJKSolver::shapeDistance<Sphere, Box>(const Sphere& s1,
                                           const Transform3f& tf1,
                                           const Box& s2,
                                           const Transform3f& tf2,
                                           FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
                                           Vec3f& normal) const {
  bool collide =
      details::boxSphereDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
  normal *= -1;
  return !collide;
}

template <>
bool GJKSolver::shapeDistance<Sphere, Cylinder>(
    const Sphere& s1, const Transform3f& tf1, const Cylinder& s2,
    const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
    Vec3f& normal) const {
  return details::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2,
                                         normal);
}

template <>
bool GJKSolver::shapeDistance<Cylinder, Sphere>(
    const Cylinder& s1, const Transform3f& tf1, const Sphere& s2,
    const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
    Vec3f& normal) const {
  return details::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1,
                                         normal);
}

template <>
bool GJKSolver::shapeDistance<Sphere, Sphere>(const Sphere& s1,
                                              const Transform3f& tf1,
                                              const Sphere& s2,
                                              const Transform3f& tf2,
                                              FCL_REAL& dist, Vec3f& p1,
                                              Vec3f& p2, Vec3f& normal) const {
  return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
}

template <>
bool GJKSolver::shapeDistance<Capsule, Capsule>(
    const Capsule& /*s1*/, const Transform3f& /*tf1*/, const Capsule& /*s2*/,
    const Transform3f& /*tf2*/, FCL_REAL& /*dist*/, Vec3f& /*p1*/,
    Vec3f& /*p2*/, Vec3f& /*normal*/) const {
  abort();
}

template <>
bool GJKSolver::shapeDistance<TriangleP, TriangleP>(
    const TriangleP& s1, const Transform3f& tf1, const TriangleP& s2,
    const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
    Vec3f& normal) const {
  const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b),
                     tf1.transform(s1.c)),
      t2(tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c));

  details::MinkowskiDiff shape;
  shape.set(&t1, &t2);

  Vec3f guess;
  guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
  support_func_guess_t support_hint;
  bool enable_penetration = true;
  details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
  initialize_gjk(gjk, shape, t1, t2, 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;
  }

  gjk.getClosestPoints(shape, p1, p2);

  if ((gjk_status == details::GJK::Valid) ||
      (gjk_status == details::GJK::Failed)) {
    // TODO On degenerated case, the closest point may be wrong
    // (i.e. an object face normal is colinear to gjk.ray
    // assert (dist == (w0 - w1).norm());
    dist = gjk.distance;

    return true;
  } else if (gjk_status == details::GJK::Inside) {
    if (enable_penetration) {
      FCL_REAL penetrationDepth = details::computePenetration(
          t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
      dist = -penetrationDepth;
      assert(dist <= 1e-6);
      // GJK says Inside when below GJK.tolerance. So non intersecting
      // triangle may trigger "Inside" and have no penetration.
      return penetrationDepth < 0;
    }
    dist = 0;
    return false;
  }
  assert(false && "should not reach this point");
  return false;
}
}  // namespace fcl

}  // namespace hpp


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2011-2014, Willow Garage, Inc.
5			* Copyright (c) 2014-2015, Open Source Robotics Foundation
6			* All rights reserved.
7			*
8			* Redistribution and use in source and binary forms, with or without
9			* modification, are permitted provided that the following conditions
10			* are met:
11			*
12			* * Redistributions of source code must retain the above copyright
13			* notice, this list of conditions and the following disclaimer.
14			* * Redistributions in binary form must reproduce the above
15			* copyright notice, this list of conditions and the following
16			* disclaimer in the documentation and/or other materials provided
17			* with the distribution.
18			* * Neither the name of Open Source Robotics Foundation nor the names of its
19			* contributors may be used to endorse or promote products derived
20			* from this software without specific prior written permission.
21			*
22			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33			* POSSIBILITY OF SUCH DAMAGE.
34			*/
35
36			/** \author Jia Pan */
37
38			#include <hpp/fcl/narrowphase/narrowphase.h>
39
40			#include <vector>
41
42			#include <hpp/fcl/shape/geometric_shapes_utility.h>
43			#include <hpp/fcl/internal/intersect.h>
44			#include "details.h"
45
46			namespace hpp {
47			namespace fcl {
48			// Shape intersect algorithms based on:
49			// - built-in function: 0
50			// - GJK: 1
51			//
52			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
53			// \| \| box \| sphere \| capsule \| cone \| cylinder \| plane \| half-space
54			// \| triangle \|
55			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
56			// \| box \| 0 \| 0 \| 1 \| 1 \| 1 \| 0 \| 0 \| 1
57			// \|
58			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
59			// \| sphere \|/////\| 0 \| 0 \| 1 \| 1 \| 0 \| 0 \| 0
60			// \|
61			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
62			// \| capsule \|/////\|////////\| 1 \| 1 \| 1 \| 0 \| 0 \| 1
63			// \|
64			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
65			// \| cone \|/////\|////////\|/////////\| 1 \| 1 \| 0 \| 0 \| 1
66			// \|
67			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
68			// \| cylinder \|/////\|////////\|/////////\|//////\| 1 \| 0 \| 0 \| 1
69			// \|
70			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
71			// \| plane \|/////\|////////\|/////////\|//////\|//////////\| 0 \| 0 \| 0
72			// \|
73			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
74			// \| half-space \|/////\|////////\|/////////\|//////\|//////////\|///////\| 0 \| 0
75			// \|
76			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
77			// \| triangle \|/////\|////////\|/////////\|//////\|//////////\|///////\|////////////\|
78			// 1 \|
79			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
80
81			#define SHAPE_INTERSECT_INVERTED(Shape1, Shape2) \
82			template <> \
83			bool GJKSolver::shapeIntersect<Shape1, Shape2>( \
84			const Shape1& s1, const Transform3f& tf1, const Shape2& s2, \
85			const Transform3f& tf2, FCL_REAL& distance_lower_bound, \
86			bool enable_penetration, Vec3f* contact_points, Vec3f* normal) const { \
87			bool res = shapeIntersect(s2, tf2, s1, tf1, distance_lower_bound, \
88			enable_penetration, contact_points, normal); \
89			(normal) = -1.0; \
90			return res; \
91			}
92
93			template <>
94			bool GJKSolver::shapeIntersect<Sphere, Capsule>(
95			const Sphere& s1, const Transform3f& tf1, const Capsule& s2,
96			const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool,
97			Vec3f* contact_points, Vec3f* normal) const {
98			return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, distance_lower_bound,
99			contact_points, normal);
100			}
101
102			SHAPE_INTERSECT_INVERTED(Capsule, Sphere)
103
104			template <>
105			bool GJKSolver::shapeIntersect<Sphere, Sphere>(
106			const Sphere& s1, const Transform3f& tf1, const Sphere& s2,
107			const Transform3f& tf2, FCL_REAL& distance_lower_bound, bool,
108			Vec3f* contact_points, Vec3f* normal) const {
109			return details::sphereSphereIntersect(s1, tf1, s2, tf2, distance_lower_bound,
110			contact_points, normal);
111			}
112
113			template <>
114		1000	bool GJKSolver::shapeIntersect<Box, Sphere>(
115			const Box& s1, const Transform3f& tf1, const Sphere& s2,
116			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
117			Vec3f* normal) const {
118	✓✗✓✗ ✓✗	1000	Vec3f ps, pb, n;
119	✓✗	1000	bool res = details::boxSphereDistance(s1, tf1, s2, tf2, distance, ps, pb, n);
120	✗✓✗✗	1000	if (normal) *normal = n;
121	✗✓✗✗	1000	if (contact_points) *contact_points = pb;
122		1000	return res;
123			}
124
125			SHAPE_INTERSECT_INVERTED(Sphere, Box)
126
127			/*
128			template<>
129			bool GJKSolver::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
130			const Box& s2, const Transform3f& tf2,
131			FCL_REAL& distance_lower_bound,
132			bool,
133			Vec3f* contact_points, Vec3f* normal)
134			const
135			{
136			return details::boxBoxIntersect(s1, tf1, s2, tf2, contact_points,
137			&distance_lower_bound, normal);
138			}
139			*/
140
141			template <>
142			bool GJKSolver::shapeIntersect<Sphere, Halfspace>(
143			const Sphere& s1, const Transform3f& tf1, const Halfspace& s2,
144			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
145			Vec3f* normal) const {
146			Vec3f p1, p2, n;
147			bool res =
148			details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
149			if (contact_points) *contact_points = p1;
150			if (normal) *normal = n;
151			return res;
152			}
153
154			SHAPE_INTERSECT_INVERTED(Halfspace, Sphere)
155
156			template <>
157			bool GJKSolver::shapeIntersect<Box, Halfspace>(
158			const Box& s1, const Transform3f& tf1, const Halfspace& s2,
159			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
160			Vec3f* normal) const {
161			Vec3f p1, p2, n;
162			bool res =
163			details::boxHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
164			if (contact_points) *contact_points = p1;
165			if (normal) *normal = n;
166			return res;
167			}
168
169			SHAPE_INTERSECT_INVERTED(Halfspace, Box)
170
171			template <>
172			bool GJKSolver::shapeIntersect<Capsule, Halfspace>(
173			const Capsule& s1, const Transform3f& tf1, const Halfspace& s2,
174			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
175			Vec3f* normal) const {
176			Vec3f p1, p2, n;
177			bool res =
178			details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
179			if (contact_points) *contact_points = p1;
180			if (normal) *normal = n;
181			return res;
182			}
183
184			SHAPE_INTERSECT_INVERTED(Halfspace, Capsule)
185
186			template <>
187			bool GJKSolver::shapeIntersect<Cylinder, Halfspace>(
188			const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2,
189			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
190			Vec3f* normal) const {
191			Vec3f p1, p2, n;
192			bool res = details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
193			p2, n);
194			if (contact_points) *contact_points = p1;
195			if (normal) *normal = n;
196			return res;
197			}
198
199			SHAPE_INTERSECT_INVERTED(Halfspace, Cylinder)
200
201			template <>
202			bool GJKSolver::shapeIntersect<Cone, Halfspace>(
203			const Cone& s1, const Transform3f& tf1, const Halfspace& s2,
204			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
205			Vec3f* normal) const {
206			Vec3f p1, p2, n;
207			bool res =
208			details::coneHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
209			if (contact_points) *contact_points = p1;
210			if (normal) *normal = n;
211			return res;
212			}
213
214			SHAPE_INTERSECT_INVERTED(Halfspace, Cone)
215
216			template <>
217			bool GJKSolver::shapeIntersect<Halfspace, Halfspace>(
218			const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2,
219			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /contact_points/,
220			Vec3f* /normal/) const {
221			Halfspace s;
222			Vec3f p, d;
223			FCL_REAL depth;
224			int ret;
225			bool res = details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret);
226			distance = -depth;
227			return res;
228			}
229
230			template <>
231			bool GJKSolver::shapeIntersect<Plane, Halfspace>(
232			const Plane& s1, const Transform3f& tf1, const Halfspace& s2,
233			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* /contact_points/,
234			Vec3f* /normal/) const {
235			Plane pl;
236			Vec3f p, d;
237			FCL_REAL depth;
238			int ret;
239			bool res =
240			details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret);
241			distance = -depth;
242			return res;
243			}
244
245			SHAPE_INTERSECT_INVERTED(Halfspace, Plane)
246
247			template <>
248			bool GJKSolver::shapeIntersect<Sphere, Plane>(
249			const Sphere& s1, const Transform3f& tf1, const Plane& s2,
250			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
251			Vec3f* normal) const {
252			Vec3f p1, p2, n;
253			bool res =
254			details::spherePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
255			if (contact_points) *contact_points = p1;
256			if (normal) *normal = n;
257			return res;
258			}
259
260			SHAPE_INTERSECT_INVERTED(Plane, Sphere)
261
262			template <>
263			bool GJKSolver::shapeIntersect<Box, Plane>(
264			const Box& s1, const Transform3f& tf1, const Plane& s2,
265			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
266			Vec3f* normal) const {
267			Vec3f p1, p2, n;
268			bool res = details::boxPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
269			if (contact_points) *contact_points = p1;
270			if (normal) *normal = n;
271			return res;
272			}
273
274			SHAPE_INTERSECT_INVERTED(Plane, Box)
275
276			template <>
277			bool GJKSolver::shapeIntersect<Capsule, Plane>(
278			const Capsule& s1, const Transform3f& tf1, const Plane& s2,
279			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
280			Vec3f* normal) const {
281			Vec3f p1, p2, n;
282			bool res =
283			details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
284			if (contact_points) *contact_points = p1;
285			if (normal) *normal = n;
286			return res;
287			}
288
289			SHAPE_INTERSECT_INVERTED(Plane, Capsule)
290
291			template <>
292			bool GJKSolver::shapeIntersect<Cylinder, Plane>(
293			const Cylinder& s1, const Transform3f& tf1, const Plane& s2,
294			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
295			Vec3f* normal) const {
296			Vec3f p1, p2, n;
297			bool res =
298			details::cylinderPlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
299			if (contact_points) *contact_points = p1;
300			if (normal) *normal = n;
301			return res;
302			}
303
304			SHAPE_INTERSECT_INVERTED(Plane, Cylinder)
305
306			template <>
307			bool GJKSolver::shapeIntersect<Cone, Plane>(
308			const Cone& s1, const Transform3f& tf1, const Plane& s2,
309			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
310			Vec3f* normal) const {
311			Vec3f p1, p2, n;
312			bool res = details::conePlaneIntersect(s1, tf1, s2, tf2, distance, p1, p2, n);
313			if (contact_points) *contact_points = p1;
314			if (normal) *normal = n;
315			return res;
316			}
317
318			SHAPE_INTERSECT_INVERTED(Plane, Cone)
319
320			template <>
321			bool GJKSolver::shapeIntersect<Plane, Plane>(
322			const Plane& s1, const Transform3f& tf1, const Plane& s2,
323			const Transform3f& tf2, FCL_REAL& distance, bool, Vec3f* contact_points,
324			Vec3f* normal) const {
325			return details::planeIntersect(s1, tf1, s2, tf2, contact_points, &distance,
326			normal);
327			}
328
329			template <>
330		34	bool GJKSolver::shapeTriangleInteraction(
331			const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
332			const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
333			Vec3f& p2, Vec3f& normal) const {
334	✓✗✓✗	34	return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1),
335	✓✗	68	tf2.transform(P2), tf2.transform(P3),
336		68	distance, p1, p2, normal);
337			}
338
339			template <>
340		12	bool GJKSolver::shapeTriangleInteraction(
341			const Halfspace& s, const Transform3f& tf1, const Vec3f& P1,
342			const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
343			FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const {
344		12	return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance,
345		12	p1, p2, normal);
346			}
347
348			template <>
349		11	bool GJKSolver::shapeTriangleInteraction(const Plane& s, const Transform3f& tf1,
350			const Vec3f& P1, const Vec3f& P2,
351			const Vec3f& P3,
352			const Transform3f& tf2,
353			FCL_REAL& distance, Vec3f& p1,
354			Vec3f& p2, Vec3f& normal) const {
355		11	return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, distance, p1,
356		11	p2, normal);
357			}
358
359			// Shape distance algorithms not using built-in GJK algorithm
360			//
361			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
362			// \| \| box \| sphere \| capsule \| cone \| cylinder \| plane \| half-space
363			// \| triangle \|
364			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
365			// \| box \| \| O \| \| \| \| \| \| \|
366			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
367			// \| sphere \|/////\| O \| O \| \| O \| \| \| \|
368			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
369			// \| capsule \|/////\|////////\| O \| \| \| \| \| \|
370			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
371			// \| cone \|/////\|////////\|/////////\| \| \| \| \| \|
372			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
373			// \| cylinder \|/////\|////////\|/////////\|//////\| \| \| \| \|
374			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
375			// \| plane \|/////\|////////\|/////////\|//////\|//////////\| \| \| \|
376			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
377			// \| half-space \|/////\|////////\|/////////\|//////\|//////////\|///////\| \| \|
378			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
379			// \| triangle \|/////\|////////\|/////////\|//////\|//////////\|///////\|////////////\|
380			// \|
381			// +------------+-----+--------+---------+------+----------+-------+------------+----------+
382
383			template <>
384		31	bool GJKSolver::shapeDistance<Sphere, Capsule>(const Sphere& s1,
385			const Transform3f& tf1,
386			const Capsule& s2,
387			const Transform3f& tf2,
388			FCL_REAL& dist, Vec3f& p1,
389			Vec3f& p2, Vec3f& normal) const {
390		31	return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
391			}
392
393			template <>
394		2	bool GJKSolver::shapeDistance<Capsule, Sphere>(const Capsule& s1,
395			const Transform3f& tf1,
396			const Sphere& s2,
397			const Transform3f& tf2,
398			FCL_REAL& dist, Vec3f& p1,
399			Vec3f& p2, Vec3f& normal) const {
400		2	return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
401			}
402
403			template <>
404			bool GJKSolver::shapeDistance<Box, Sphere>(const Box& s1,
405			const Transform3f& tf1,
406			const Sphere& s2,
407			const Transform3f& tf2,
408			FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
409			Vec3f& normal) const {
410			return !details::boxSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
411			}
412
413			template <>
414		45	bool GJKSolver::shapeDistance<Sphere, Box>(const Sphere& s1,
415			const Transform3f& tf1,
416			const Box& s2,
417			const Transform3f& tf2,
418			FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
419			Vec3f& normal) const {
420			bool collide =
421		45	details::boxSphereDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
422	✓✗	45	normal *= -1;
423		45	return !collide;
424			}
425
426			template <>
427			bool GJKSolver::shapeDistance<Sphere, Cylinder>(
428			const Sphere& s1, const Transform3f& tf1, const Cylinder& s2,
429			const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
430			Vec3f& normal) const {
431			return details::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2,
432			normal);
433			}
434
435			template <>
436			bool GJKSolver::shapeDistance<Cylinder, Sphere>(
437			const Cylinder& s1, const Transform3f& tf1, const Sphere& s2,
438			const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
439			Vec3f& normal) const {
440			return details::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1,
441			normal);
442			}
443
444			template <>
445		24	bool GJKSolver::shapeDistance<Sphere, Sphere>(const Sphere& s1,
446			const Transform3f& tf1,
447			const Sphere& s2,
448			const Transform3f& tf2,
449			FCL_REAL& dist, Vec3f& p1,
450			Vec3f& p2, Vec3f& normal) const {
451		24	return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2, normal);
452			}
453
454			template <>
455			bool GJKSolver::shapeDistance<Capsule, Capsule>(
456			const Capsule& /s1/, const Transform3f& /tf1/, const Capsule& /s2/,
457			const Transform3f& /tf2/, FCL_REAL& /dist/, Vec3f& /p1/,
458			Vec3f& /p2/, Vec3f& /normal/) const {
459			abort();
460			}
461
462			template <>
463		29203802	bool GJKSolver::shapeDistance<TriangleP, TriangleP>(
464			const TriangleP& s1, const Transform3f& tf1, const TriangleP& s2,
465			const Transform3f& tf2, FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
466			Vec3f& normal) const {
467	✓✗✓✗	29203802	const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b),
468	✓✗✓✗	87611406	tf1.transform(s1.c)),
469	✓✗✓✗ ✓✗✓✗	58407604	t2(tf2.transform(s2.a), tf2.transform(s2.b), tf2.transform(s2.c));
470
471	✓✗	58407604	details::MinkowskiDiff shape;
472	✓✗	29203802	shape.set(&t1, &t2);
473
474	✓✗	29203802	Vec3f guess;
475	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	29203802	guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
476	✓✗	29203802	support_func_guess_t support_hint;
477		29203802	bool enable_penetration = true;
478	✓✗	29203802	details::GJK gjk((unsigned int)gjk_max_iterations, gjk_tolerance);
479	✓✗	29203802	initialize_gjk(gjk, shape, t1, t2, guess, support_hint);
480
481	✓✗	29203802	details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
482	✓✗	29203802	if (gjk_initial_guess == GJKInitialGuess::CachedGuess \|\|
483	✗✓	29203802	enable_cached_guess) {
484			cached_guess = gjk.getGuessFromSimplex();
485			support_func_cached_guess = gjk.support_hint;
486			}
487
488	✓✗	29203802	gjk.getClosestPoints(shape, p1, p2);
489
490	✓✓✗✓	29203802	if ((gjk_status == details::GJK::Valid) \|\|
491			(gjk_status == details::GJK::Failed)) {
492			// TODO On degenerated case, the closest point may be wrong
493			// (i.e. an object face normal is colinear to gjk.ray
494			// assert (dist == (w0 - w1).norm());
495		29182783	dist = gjk.distance;
496
497		29182783	return true;
498	✓✗	21019	} else if (gjk_status == details::GJK::Inside) {
499	✓✗	21019	if (enable_penetration) {
500	✓✗	21019	FCL_REAL penetrationDepth = details::computePenetration(
501			t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
502		21019	dist = -penetrationDepth;
503	✗✓	21019	assert(dist <= 1e-6);
504			// GJK says Inside when below GJK.tolerance. So non intersecting
505			// triangle may trigger "Inside" and have no penetration.
506		21019	return penetrationDepth < 0;
507			}
508			dist = 0;
509			return false;
510			}
511			assert(false && "should not reach this point");
512			return false;
513			}
514			} // namespace fcl
515
516			} // namespace hpp