Commit 70f44363 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add distance computation for OBB based BVH.

parent 68412e55
......@@ -137,6 +137,8 @@ public:
virtual ~DistanceTraversalNodeBase();
/// @brief BV test between b1 and b2
/// \return a lower bound of the distance between the two BV.
/// \note except for OBB, this method returns the distance.
virtual FCL_REAL BVTesting(int b1, int b2) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs
......
......@@ -298,6 +298,32 @@ public:
Vec3f T;
};
namespace details
{
template<typename BV> struct DistanceTraversalBVTesting_impl
{
static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
{
return b1.distance(b2);
}
};
template<> struct DistanceTraversalBVTesting_impl<OBB>
{
static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
{
FCL_REAL sqrDistLowerBound;
CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (b1.overlap(b2, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt (sqrDistLowerBound);
}
};
} // namespace details
/// @brief Traversal node for distance computation between BVH models
template<typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
......@@ -367,7 +393,8 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return model1->getBV(b1).distance(model2->getBV(b2));
return details::DistanceTraversalBVTesting_impl<BV>
::run (model1->getBV(b1), model2->getBV(b2));
}
/// @brief The first BVH model
......
......@@ -379,6 +379,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
*/
distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
......@@ -388,7 +389,6 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
*/
distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
......@@ -448,6 +448,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB, NarrowPhaseSolver>;
distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
......
......@@ -198,7 +198,36 @@ BOOST_AUTO_TEST_CASE(mesh_distance)
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
......
Supports Markdown
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