Skip to content

Commit

Permalink
Add custom sphere-box collision and distance tests
Browse files Browse the repository at this point in the history 
By default, the GJK solver was being used for performing distance queries
between box's 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-box 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_box.cpp)
  - Borrows the CompareMatrices functionality from Drake and adds it to FCL.
  - Adds the custom sphere-box collision (sphere_box.h and sphere_box-inl.h)
  - Adds *extensive* unit tests on the custom algorithm.
  - Ties the custom algorithm into the libccd and indep GJK solvers.
  - Remove a useless conflicting test from test_fcl_collision (it's only
    useless in retrospect). And its formulation can't help but become
    corrupt.
  - Update *other* tests that otherwise depend on box-sphere collision.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI committed Jul 31, 2018
1 parent 157f34f commit 8fb2ce0
Show file tree
Hide file tree
Showing 14 changed files with 1,670 additions and 114 deletions.
5 changes: 5 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,10 @@

* Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156)

* Narrowphase

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

* Distance

* Added distance request option for computing exact negative distance: [#172](https://github.com/flexible-collision-library/fcl/pull/172)
Expand All @@ -26,6 +30,7 @@
* Added CMake targets for generating API documentation: [#174](https://github.com/flexible-collision-library/fcl/pull/174)
* Enabled build with SSE option by default: [#159](https://github.com/flexible-collision-library/fcl/pull/159)
* Added missing copyright headers: [#149](https://github.com/flexible-collision-library/fcl/pull/149)
* Added test utility for performing equality between Eigen matrix-types (`CompareMatrices` in `test/eign_matrix_compare.h`): [#316](https://github.com/flexible-collision-library/fcl/pull/316)

### FCL 0.5.0 (2016-07-19)

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 @@ -49,6 +49,7 @@
#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk.h"
#include "fcl/narrowphase/detail/convexity_based_algorithm/epa.h"
#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_sphere.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
Expand Down Expand Up @@ -181,7 +182,7 @@ bool GJKSolver_indep<S>::shapeIntersect(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | O | | | | | | O | O | |
// | box | O | O | | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand Down Expand Up @@ -246,6 +247,8 @@ FCL_GJK_INDEP_SHAPE_INTERSECT(Box, detail::boxBoxIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)

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 @@ -670,7 +673,7 @@ bool GJKSolver_indep<S>::shapeSignedDistance(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | | | | | | | | | |
// | box | | O | | | | | | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | | | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand All @@ -689,6 +692,42 @@ bool GJKSolver_indep<S>::shapeSignedDistance(
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

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

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

//==============================================================================
template<typename S>
struct ShapeDistanceIndepImpl<S, Sphere<S>, Capsule<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 @@ -46,6 +46,7 @@

#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h"
#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_sphere.h"
#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h"
Expand Down Expand Up @@ -177,7 +178,7 @@ bool GJKSolver_libccd<S>::shapeIntersect(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | O | | | | | | O | O | |
// | box | O | O | | | | | O | O | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | O | O | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand Down Expand Up @@ -242,6 +243,8 @@ FCL_GJK_LIBCCD_SHAPE_INTERSECT(Box, detail::boxBoxIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)

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 @@ -651,7 +654,7 @@ bool GJKSolver_libccd<S>::shapeDistance(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box | | | | | | | | | |
// | box | | O | | | | | | | |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere |/////| O | | O | | | | | O |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
Expand All @@ -670,6 +673,42 @@ bool GJKSolver_libccd<S>::shapeDistance(
// | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

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

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

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

0 comments on commit 8fb2ce0

Please sign in to comment.