Skip to content

Commit cca1208

Browse files
SeanCurtis-TRISeanCurtis-TRI
authored
Add primitive cylinder-sphere collision and distance query (#321)
* 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.
1 parent a278363 commit cca1208

File tree

10 files changed

+1729
-4
lines changed

10 files changed

+1729
-4
lines changed

‎CHANGELOG.md

+1
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
* Narrowphase
2121

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

2425
* Distance
2526

‎include/fcl/narrowphase/detail/gjk_solver_indep-inl.h

+41-2
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,7 @@
5151
#include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h"
5252
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h"
5353
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h"
54+
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h"
5455
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h"
5556
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
5657
#include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h"
@@ -184,7 +185,7 @@ bool GJKSolver_indep<S>::shapeIntersect(
184185
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
185186
// | box | O | O | | | | | O | O | |
186187
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
187-
// | sphere |/////| O | | O | | | O | O | O |
188+
// | sphere |/////| O | | O | | O | O | O | O |
188189
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
189190
// | ellipsoid |/////|////////| | | | | O | O | |
190191
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
@@ -249,6 +250,8 @@ FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInters
249250

250251
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)
251252

253+
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect)
254+
252255
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
253256
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
254257
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect)
@@ -675,7 +678,7 @@ bool GJKSolver_indep<S>::shapeSignedDistance(
675678
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
676679
// | box | | O | | | | | | | |
677680
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
678-
// | sphere |/////| O | | O | | | | | O |
681+
// | sphere |/////| O | | O | | O | | | O |
679682
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
680683
// | ellipsoid |/////|////////| | | | | | | |
681684
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
@@ -764,6 +767,42 @@ struct ShapeDistanceIndepImpl<S, Capsule<S>, Sphere<S>>
764767
}
765768
};
766769

770+
//==============================================================================
771+
template<typename S>
772+
struct ShapeDistanceIndepImpl<S, Sphere<S>, Cylinder<S>>
773+
{
774+
static bool run(
775+
const GJKSolver_indep<S>& /*gjkSolver*/,
776+
const Sphere<S>& s1,
777+
const Transform3<S>& tf1,
778+
const Cylinder<S>& s2,
779+
const Transform3<S>& tf2,
780+
S* dist,
781+
Vector3<S>* p1,
782+
Vector3<S>* p2)
783+
{
784+
return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2);
785+
}
786+
};
787+
788+
//==============================================================================
789+
template<typename S>
790+
struct ShapeDistanceIndepImpl<S, Cylinder<S>, Sphere<S>>
791+
{
792+
static bool run(
793+
const GJKSolver_indep<S>& /*gjkSolver*/,
794+
const Cylinder<S>& s1,
795+
const Transform3<S>& tf1,
796+
const Sphere<S>& s2,
797+
const Transform3<S>& tf2,
798+
S* dist,
799+
Vector3<S>* p1,
800+
Vector3<S>* p2)
801+
{
802+
return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1);
803+
}
804+
};
805+
767806
//==============================================================================
768807
template<typename S>
769808
struct ShapeDistanceIndepImpl<S, Sphere<S>, Sphere<S>>

‎include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h

+41-2
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@
4848
#include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h"
4949
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h"
5050
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h"
51+
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h"
5152
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.h"
5253
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
5354
#include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h"
@@ -180,7 +181,7 @@ bool GJKSolver_libccd<S>::shapeIntersect(
180181
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
181182
// | box | O | O | | | | | O | O | |
182183
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
183-
// | sphere |/////| O | | O | | | O | O | O |
184+
// | sphere |/////| O | | O | | O | O | O | O |
184185
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
185186
// | ellipsoid |/////|////////| | | | | O | O | TODO |
186187
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
@@ -245,6 +246,8 @@ FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleInter
245246

246247
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)
247248

249+
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Cylinder, detail::sphereCylinderIntersect)
250+
248251
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
249252
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
250253
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Box, Halfspace, detail::boxHalfspaceIntersect)
@@ -656,7 +659,7 @@ bool GJKSolver_libccd<S>::shapeDistance(
656659
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
657660
// | box | | O | | | | | | | |
658661
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
659-
// | sphere |/////| O | | O | | | | | O |
662+
// | sphere |/////| O | | O | | O | | | O |
660663
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
661664
// | ellipsoid |/////|////////| | | | | | | |
662665
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
@@ -745,6 +748,42 @@ struct ShapeDistanceLibccdImpl<S, Capsule<S>, Sphere<S>>
745748
}
746749
};
747750

751+
//==============================================================================
752+
template<typename S>
753+
struct ShapeDistanceLibccdImpl<S, Sphere<S>, Cylinder<S>>
754+
{
755+
static bool run(
756+
const GJKSolver_libccd<S>& /*gjkSolver*/,
757+
const Sphere<S>& s1,
758+
const Transform3<S>& tf1,
759+
const Cylinder<S>& s2,
760+
const Transform3<S>& tf2,
761+
S* dist,
762+
Vector3<S>* p1,
763+
Vector3<S>* p2)
764+
{
765+
return detail::sphereCylinderDistance(s1, tf1, s2, tf2, dist, p1, p2);
766+
}
767+
};
768+
769+
//==============================================================================
770+
template<typename S>
771+
struct ShapeDistanceLibccdImpl<S, Cylinder<S>, Sphere<S>>
772+
{
773+
static bool run(
774+
const GJKSolver_libccd<S>& /*gjkSolver*/,
775+
const Cylinder<S>& s1,
776+
const Transform3<S>& tf1,
777+
const Sphere<S>& s2,
778+
const Transform3<S>& tf2,
779+
S* dist,
780+
Vector3<S>* p1,
781+
Vector3<S>* p2)
782+
{
783+
return detail::sphereCylinderDistance(s2, tf2, s1, tf1, dist, p2, p1);
784+
}
785+
};
786+
748787
//==============================================================================
749788
template<typename S>
750789
struct ShapeDistanceLibccdImpl<S, Sphere<S>, Sphere<S>>

0 commit comments

Comments
 (0)