Skip to content

Commit

Permalink
Add primitive cylinder-sphere collision and distance query (#321)
Browse files Browse the repository at this point in the history 
* Add custom sphere-cylinder collision and distance tests

By default, the GJK solver was being used for performing distance queries
between cylinders and spheres. For small features, the answer was being
dominated by the iterative tolerance and producing wildly problematic
values. The logical thing to do is to perform sphere-cylinder collisions
using knowledge of the primitives.

This commit adds the following:
  - A new test illustrating the error of GJK is used
    (see test_fcl_sphere_cylinder.cpp)
  - Adds the custom sphere-cylinder collision/distance
    (sphere_cylinder.h and sphere_cylinder-inl.h)
  - Adds *extensive* unit tests on the custom algorithm.
  - Ties the custom algorithm into the libccd and indep GJK solvers.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI authored Aug 8, 2018
1 parent a278363 commit cca1208
Show file tree
Hide file tree
Showing 10 changed files with 1,729 additions and 4 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
* Narrowphase

* Add custom sphere-box collision and distance algorithms for both solvers: [#316](https://github.com/flexible-collision-library/fcl/pull/316)
* Add custom sphere-cylinder collision and distance algorithms for both solvers: [#321](https://github.com/flexible-collision-library/fcl/pull/321)

* Distance

Expand Down
43 changes: 41 additions & 2 deletions include/fcl/narrowphase/detail/gjk_solver_indep-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
#include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h"
Expand Down Expand Up @@ -184,7 +185,7 @@ bool GJKSolver_indep<S>::shapeIntersect(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | O | O | | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | O | O | O |
// | sphere |/////| O | | O | | O | O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid |/////|////////| | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand Down Expand Up @@ -249,6 +250,8 @@ FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInters

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect)
Expand Down Expand Up @@ -675,7 +678,7 @@ bool GJKSolver_indep<S>::shapeSignedDistance(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | | O | | | | | | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | | | O |
// | sphere |/////| O | | O | | O | | | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid |/////|////////| | | | | | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand Down Expand Up @@ -764,6 +767,42 @@ struct ShapeDistanceIndepImpl<S, Capsule<S>, Sphere<S>>
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceIndepImpl<S, Sphere<S>, Cylinder<S>>
{
static bool run(
const GJKSolver_indep<S>& /*gjkSolver*/,
const Sphere<S>& s1,
const Transform3<S>& tf1,
const Cylinder<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceIndepImpl<S, Cylinder<S>, Sphere<S>>
{
static bool run(
const GJKSolver_indep<S>& /*gjkSolver*/,
const Cylinder<S>& s1,
const Transform3<S>& tf1,
const Sphere<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1);
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceIndepImpl<S, Sphere<S>, Sphere<S>>
Expand Down
43 changes: 41 additions & 2 deletions include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
#include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h"
Expand Down Expand Up @@ -180,7 +181,7 @@ bool GJKSolver_libccd<S>::shapeIntersect(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | O | O | | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | O | O | O |
// | sphere |/////| O | | O | | O | O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid |/////|////////| | | | | O | O | TODO |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand Down Expand Up @@ -245,6 +246,8 @@ FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInter

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect)
Expand Down Expand Up @@ -656,7 +659,7 @@ bool GJKSolver_libccd<S>::shapeDistance(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | | O | | | | | | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | | | O |
// | sphere |/////| O | | O | | O | | | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid |/////|////////| | | | | | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand Down Expand Up @@ -745,6 +748,42 @@ struct ShapeDistanceLibccdImpl<S, Capsule<S>, Sphere<S>>
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceLibccdImpl<S, Sphere<S>, Cylinder<S>>
{
static bool run(
const GJKSolver_libccd<S>& /*gjkSolver*/,
const Sphere<S>& s1,
const Transform3<S>& tf1,
const Cylinder<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceLibccdImpl<S, Cylinder<S>, Sphere<S>>
{
static bool run(
const GJKSolver_libccd<S>& /*gjkSolver*/,
const Cylinder<S>& s1,
const Transform3<S>& tf1,
const Sphere<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1);
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceLibccdImpl<S, Sphere<S>, Sphere<S>>
Expand Down
Loading

0 comments on commit cca1208

Please sign in to comment.