Skip to content
Snippets Groups Projects
Commit bd711b29 authored by Jeongseok Lee's avatar Jeongseok Lee
Browse files

Merge pull request #53 from dartsim/reverse_shape_algorithms 

Use same shape algorithms for the reverse shape pair
parents 46edbc84 9ed22c36
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
include/fcl/narrowphase/narrowphase.h
+ 149
− 29
View file @ bd711b29
......@@ -249,6 +249,11 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const T
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
const Sphere &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
......@@ -266,26 +271,51 @@ bool GJKSolver_libccd::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
const Cylinder& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
const Cone& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
......@@ -296,35 +326,60 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
const Cylinder& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
bool GJKSolver_libccd::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
const Cone& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
......@@ -356,12 +411,23 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tr
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
template<>
bool GJKSolver_libccd::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
template<>
bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
......@@ -373,12 +439,6 @@ template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
template<>
bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver_indep
......@@ -732,18 +792,24 @@ struct GJKSolver_indep
mutable Vec3f cached_guess;
};
/// @brief Fast implementation for sphere-capsule collision
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
const Capsule &s2, const Transform3f& tf2,
bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Sphere>(const Capsule &s1, const Transform3f& tf1,
const Sphere &s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for box-box collision
/// @brief Fast implementation for box-box collision
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
......@@ -754,26 +820,51 @@ bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Box>(const Halfspace& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Halfspace>(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Capsule>(const Halfspace& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Halfspace>(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>(const Halfspace& s1, const Transform3f& tf1,
const Cylinder& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
const Cone& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
......@@ -784,35 +875,60 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Plane>(const Sphere& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
const Cylinder& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
const Cone& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transform3f& tf1,
......@@ -820,15 +936,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Plane>(const Plane& s1, const Transf
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
template<>
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-triangle collision
template<>
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
......@@ -837,18 +954,16 @@ template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
/// @brief Fast implementation for sphere-capsule distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
template<>
bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
template<>
bool GJKSolver_indep::shapeDistance<Capsule, Sphere>(const Capsule& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-sphere distance
template<>
......@@ -856,19 +971,24 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
// @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments.
template<>
bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const;
}
#endif
src/narrowphase/narrowphase.cpp
+ 372
− 49
View file @ bd711b29
This diff is collapsed.
test/test_fcl_geometric_shapes.cpp
+ 174
− 0
View file @ bd711b29
......@@ -2752,3 +2752,177 @@ BOOST_AUTO_TEST_CASE(conecone)
template<typename S1, typename S2>
void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance)
{
Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
Vec3f contactA;
Vec3f contactB;
FCL_REAL depthA;
FCL_REAL depthB;
Vec3f normalA;
Vec3f normalB;
bool resA;
bool resB;
const double tol = 1e-6;
resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK(contactA.equal(contactB, tol)); // contact point should be same
BOOST_CHECK(normalA.equal(-normalB, tol)); // normal should be opposite
BOOST_CHECK_CLOSE(depthA, depthB, tol); // penetration depth should be same
resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK(contactA.equal(contactB, tol));
BOOST_CHECK(normalA.equal(-normalB, tol));
BOOST_CHECK_CLOSE(depthA, depthB, tol);
}
BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes)
{
// This test check whether a shape intersection algorithm is called for the
// reverse case as well. For example, if FCL has sphere-capsule intersection
// algorithm, then this algorithm should be called for capsule-sphere case.
// Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
Box box(10, 10, 10);
Sphere sphere(5);
Capsule capsule(5, 10);
Cone cone(5, 10);
Cylinder cylinder(5, 10);
Plane plane(Vec3f(), 0.0);
Halfspace halfspace(Vec3f(), 0.0);
// Use sufficiently short distance so that all the primitive shapes can intersect
FCL_REAL distance = 5.0;
// If new shape intersection algorithm is added for two distinct primitive
// shapes, uncomment associated lines. For example, box-sphere intersection
// algorithm is added, then uncomment box-sphere.
// testReversibleShapeIntersection(box, sphere, distance);
// testReversibleShapeIntersection(box, capsule, distance);
// testReversibleShapeIntersection(box, cone, distance);
// testReversibleShapeIntersection(box, cylinder, distance);
testReversibleShapeIntersection(box, plane, distance);
testReversibleShapeIntersection(box, halfspace, distance);
testReversibleShapeIntersection(sphere, capsule, distance);
// testReversibleShapeIntersection(sphere, cone, distance);
// testReversibleShapeIntersection(sphere, cylinder, distance);
testReversibleShapeIntersection(sphere, plane, distance);
testReversibleShapeIntersection(sphere, halfspace, distance);
// testReversibleShapeIntersection(capsule, cone, distance);
// testReversibleShapeIntersection(capsule, cylinder, distance);
testReversibleShapeIntersection(capsule, plane, distance);
testReversibleShapeIntersection(capsule, halfspace, distance);
// testReversibleShapeIntersection(cone, cylinder, distance);
testReversibleShapeIntersection(cone, plane, distance);
testReversibleShapeIntersection(cone, halfspace, distance);
testReversibleShapeIntersection(cylinder, plane, distance);
testReversibleShapeIntersection(cylinder, halfspace, distance);
testReversibleShapeIntersection(plane, halfspace, distance);
}
template<typename S1, typename S2>
void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
{
Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
FCL_REAL distA;
FCL_REAL distB;
Vec3f p1A;
Vec3f p1B;
Vec3f p2A;
Vec3f p2B;
bool resA;
bool resB;
const double tol = 1e-6;
resA = solver1.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
resB = solver1.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
BOOST_CHECK(p1A.equal(p2B, tol)); // closest points should in reverse order
BOOST_CHECK(p2A.equal(p1B, tol));
resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol);
BOOST_CHECK(p1A.equal(p2B, tol));
BOOST_CHECK(p2A.equal(p1B, tol));
}
BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
{
// This test check whether a shape distance algorithm is called for the
// reverse case as well. For example, if FCL has sphere-capsule distance
// algorithm, then this algorithm should be called for capsule-sphere case.
// Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
Box box(10, 10, 10);
Sphere sphere(5);
Capsule capsule(5, 10);
Cone cone(5, 10);
Cylinder cylinder(5, 10);
Plane plane(Vec3f(), 0.0);
Halfspace halfspace(Vec3f(), 0.0);
// Use sufficiently long distance so that all the primitive shapes CANNOT intersect
FCL_REAL distance = 15.0;
// If new shape distance algorithm is added for two distinct primitive
// shapes, uncomment associated lines. For example, box-sphere intersection
// algorithm is added, then uncomment box-sphere.
// testReversibleShapeDistance(box, sphere, distance);
// testReversibleShapeDistance(box, capsule, distance);
// testReversibleShapeDistance(box, cone, distance);
// testReversibleShapeDistance(box, cylinder, distance);
// testReversibleShapeDistance(box, plane, distance);
// testReversibleShapeDistance(box, halfspace, distance);
testReversibleShapeDistance(sphere, capsule, distance);
// testReversibleShapeDistance(sphere, cone, distance);
// testReversibleShapeDistance(sphere, cylinder, distance);
// testReversibleShapeDistance(sphere, plane, distance);
// testReversibleShapeDistance(sphere, halfspace, distance);
// testReversibleShapeDistance(capsule, cone, distance);
// testReversibleShapeDistance(capsule, cylinder, distance);
// testReversibleShapeDistance(capsule, plane, distance);
// testReversibleShapeDistance(capsule, halfspace, distance);
// testReversibleShapeDistance(cone, cylinder, distance);
// testReversibleShapeDistance(cone, plane, distance);
// testReversibleShapeDistance(cone, halfspace, distance);
// testReversibleShapeDistance(cylinder, plane, distance);
// testReversibleShapeDistance(cylinder, halfspace, distance);
// testReversibleShapeDistance(plane, halfspace, distance);
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment