Commit 16a9f62f authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add lower bound on distance in collision detection (preliminary)

  - add a Boolean value in CollisionRequest to trigger this option,
  - if selected, fcl::collision function will compute a lower bound
    on the distance between the two objects if they do not collide.
parent fb8bd312
......@@ -182,6 +182,9 @@ struct CollisionRequest
/// @brief whether the contact information (normal, penetration depth and contact position) will return
bool enable_contact;
/// Whether a lower bound on distance is returned when objects are disjoint
bool enable_distance_lower_bound;
/// @brief The maximum number of cost sources will return
size_t num_max_cost_sources;
......@@ -202,6 +205,7 @@ struct CollisionRequest
CollisionRequest(size_t num_max_contacts_ = 1,
bool enable_contact_ = false,
bool enable_distance_lower_bound = false,
size_t num_max_cost_sources_ = 1,
bool enable_cost_ = false,
bool use_approximate_cost_ = true,
......@@ -232,6 +236,10 @@ private:
public:
Vec3f cached_gjk_guess;
/// Lower bound on distance between objects if they are disjoint
/// \note computed only on request.
FCL_REAL distance_lower_bound;
public:
CollisionResult()
{
......
......@@ -274,41 +274,49 @@ public:
return res.timesTranspose(*this);
}
// (1 0 0)^T (*this)^T v
inline U transposeDotX(const Vec3fX<S>& v) const
{
return data.transposeDot(0, v.data);
}
// (0 1 0)^T (*this)^T v
inline U transposeDotY(const Vec3fX<S>& v) const
{
return data.transposeDot(1, v.data);
}
// (0 0 1)^T (*this)^T v
inline U transposeDotZ(const Vec3fX<S>& v) const
{
return data.transposeDot(2, v.data);
}
// (\delta_{i3})^T (*this)^T v
inline U transposeDot(size_t i, const Vec3fX<S>& v) const
{
return data.transposeDot(i, v.data);
}
// (1 0 0)^T (*this) v
inline U dotX(const Vec3fX<S>& v) const
{
return data.dot(0, v.data);
}
// (0 1 0)^T (*this) v
inline U dotY(const Vec3fX<S>& v) const
{
return data.dot(1, v.data);
}
// (0 0 1)^T (*this) v
inline U dotZ(const Vec3fX<S>& v) const
{
return data.dot(2, v.data);
}
// (\delta_{i3})^T (*this) v
inline U dot(size_t i, const Vec3fX<S>& v) const
{
return data.dot(i, v.data);
......
......@@ -298,6 +298,205 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
}
// B, T orientation and position of 2nd OBB in frame of 1st OBB,
// a extent of 1st OBB,
// b extent of 2nd OBB.
bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
const Vec3f& a, const Vec3f& b,
FCL_REAL& squaredLowerBoundDistance)
{
FCL_REAL t, s;
const FCL_REAL reps = 1e-6;
FCL_REAL diff;
Matrix3f Bf = abs(B);
Bf += reps;
squaredLowerBoundDistance = 0;
// if any of these tests are one-sided, then the polyhedra are disjoint
// A1 x A2 = A0
t = ((T[0] < 0.0) ? -T[0] : T[0]);
diff = t - (a[0] + Bf.dotX(b));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
// A2 x A0 = A1
t = ((T[1] < 0.0) ? -T[1] : T[1]);
diff = t - (a[1] + Bf.dotY(b));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
// A0 x A1 = A2
t =((T[2] < 0.0) ? -T[2] : T[2]);
diff = t - (a[2] + Bf.dotZ(b));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
if (squaredLowerBoundDistance > 0)
return true;
// B1 x B2 = B0
s = B.transposeDotX(T);
t = ((s < 0.0) ? -s : s);
diff = t - (b[0] + Bf.transposeDotX(a));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
// B2 x B0 = B1
s = B.transposeDotY(T);
t = ((s < 0.0) ? -s : s);
diff = t - (b[1] + Bf.transposeDotY(a));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
// B0 x B1 = B2
s = B.transposeDotZ(T);
t = ((s < 0.0) ? -s : s);
diff = t - (b[2] + Bf.transposeDotZ(a));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
if (squaredLowerBoundDistance > 0)
return true;
// A0 x B0
s = T[2] * B(1, 0) - T[1] * B(2, 0);
t = ((s < 0.0) ? -s : s);
diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) +
b[1] * Bf(0, 2) + b[2] * Bf(0, 1));
// We need to divide by the norm || A0 x B0 ||
// As ||A0|| = ||B0|| = 1,
// 2 2
// || A0 x B0 || + (A0 | B0) = 1
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (0,0) * Bf (0,0);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A0 x B1
s = T[2] * B(1, 1) - T[1] * B(2, 1);
t = ((s < 0.0) ? -s : s);
diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) +
b[0] * Bf(0, 2) + b[2] * Bf(0, 0));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (0,1) * Bf (0,1);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A0 x B2
s = T[2] * B(1, 2) - T[1] * B(2, 2);
t = ((s < 0.0) ? -s : s);
diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) +
b[0] * Bf(0, 1) + b[1] * Bf(0, 0));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (0,2) * Bf (0,2);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A1 x B0
s = T[0] * B(2, 0) - T[2] * B(0, 0);
t = ((s < 0.0) ? -s : s);
diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) +
b[1] * Bf(1, 2) + b[2] * Bf(1, 1));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (1,0) * Bf (1,0);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A1 x B1
s = T[0] * B(2, 1) - T[2] * B(0, 1);
t = ((s < 0.0) ? -s : s);
diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) +
b[0] * Bf(1, 2) + b[2] * Bf(1, 0));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (1,1) * Bf (1,1);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A1 x B2
s = T[0] * B(2, 2) - T[2] * B(0, 2);
t = ((s < 0.0) ? -s : s);
diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) +
b[0] * Bf(1, 1) + b[1] * Bf(1, 0));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (1,2) * Bf (1,2);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A2 x B0
s = T[1] * B(0, 0) - T[0] * B(1, 0);
t = ((s < 0.0) ? -s : s);
diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) +
b[1] * Bf(2, 2) + b[2] * Bf(2, 1));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (2,0) * Bf (2,0);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A2 x B1
s = T[1] * B(0, 1) - T[0] * B(1, 1);
t = ((s < 0.0) ? -s : s);
diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) +
b[0] * Bf(2, 2) + b[2] * Bf(2, 0));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (2,1) * Bf (2,1);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
// A2 x B2
s = T[1] * B(0, 2) - T[0] * B(1, 2);
t = ((s < 0.0) ? -s : s);
diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) +
b[0] * Bf(2, 1) + b[1] * Bf(2, 0));
if (diff > 0) {
FCL_REAL sinus2 = 1 - Bf (2,2) * Bf (2,2);
assert (sinus2 > 0);
squaredLowerBoundDistance = diff * diff / sinus2;
return true;
}
return false;
}
bool OBB::overlap(const OBB& other) const
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, CNRS
* Author: Florent Lamiraux
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of CNRS nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef FCL_SRC_OBB_H
# define FCL_SRC_OBB_H
namespace fcl
{
bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
const Vec3f& a, const Vec3f& b,
FCL_REAL& squaredLowerBoundDistance);
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a,
const Vec3f& b);
} // namespace fcl
#endif // FCL_SRC_OBB_H
......@@ -852,10 +852,14 @@ bool RSS::overlap(const RSS& other) const
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
{
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
......
......@@ -74,7 +74,7 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
nsolver = new NarrowPhaseSolver();
const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
result.distance_lower_bound = -1;
std::size_t res;
if(request.num_max_contacts == 0)
{
......
......@@ -41,7 +41,7 @@
#include "fcl/traversal/traversal_node_setup.h"
#include "fcl/collision_node.h"
#include "fcl/narrowphase/narrowphase.h"
#include "distance_func_matrix.h"
namespace fcl
{
......@@ -200,6 +200,25 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
{
if(request.isSatisfied(result)) return result.numContacts();
if (request.enable_distance_lower_bound) {
DistanceResult distanceResult;
DistanceRequest distanceRequest (request.enable_contact);
FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, NarrowPhaseSolver>
(o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
if (distance <= 0) {
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
contact.normal = (p2-p1)/(p2-p1).length ();
result.addContact (contact);
return 1;
}
result.distance_lower_bound = distance;
return 0;
}
ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
......
......@@ -346,16 +346,15 @@ namespace fcl {
(o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
if (distance <= 0) {
if (request.enable_contact) {
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
contact.normal = (p2-p1)/(p2-p1).length ();
result.addContact (contact);
}
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
contact.normal = (p2-p1)/(p2-p1).length ();
result.addContact (contact);
return 1;
}
result.distance_lower_bound = distance;
return 0;
}
......
......@@ -27,6 +27,8 @@ add_fcl_test(test_fcl_box_box_distance test_fcl_box_box_distance.cpp)
add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp)
add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp)
add_fcl_test(test_fcl_obb test_fcl_obb.cpp)
#add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, CNRS
* Author: Florent Lamiraux
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of CNRS nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#define BOOST_TEST_MODULE "FCL_DISTANCE_OBB"
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include <boost/test/included/unit_test.hpp>
#include <fcl/narrowphase/narrowphase.h>
#include "../src/BV/OBB.h"
#include "../src/distance_func_matrix.h"
using fcl::FCL_REAL;
struct Sample
{
fcl::Matrix3f R;
fcl::Vec3f T;
fcl::Vec3f extent1;
fcl::Vec3f extent2;
};
struct Result
{
bool overlap;
FCL_REAL distance;
};
BOOST_AUTO_TEST_CASE(obb_overlap)
{
static const size_t nbSamples = 10000;
Sample sample [nbSamples];
FCL_REAL range = 2;
FCL_REAL sumDistance = 0, sumDistanceLowerBound = 0;
for (std::size_t i=0; i<nbSamples; ++i) {
// sample unit quaternion
FCL_REAL u1 = (FCL_REAL) rand() / RAND_MAX;
FCL_REAL u2 = (FCL_REAL) rand() / RAND_MAX;
FCL_REAL u3 = (FCL_REAL) rand() / RAND_MAX;
FCL_REAL q1 = sqrt (1-u1)*sin(2*M_PI*u2);
FCL_REAL q2 = sqrt (1-u1)*cos(2*M_PI*u2);
FCL_REAL q3 = sqrt (u1) * sin(2*M_PI*u3);
FCL_REAL q4 = sqrt (u1) * cos(2*M_PI*u3);
fcl::Quaternion3f (q1, q2, q3, q4).toRotation (sample [i].R);
// sample translation
sample [i].T = fcl::Vec3f ((FCL_REAL) range * rand() / RAND_MAX,
(FCL_REAL) range * rand() / RAND_MAX,
(FCL_REAL) range * rand() / RAND_MAX);
// sample extents
sample [i].extent1 = fcl::Vec3f ((FCL_REAL) rand() / RAND_MAX,
(FCL_REAL) rand() / RAND_MAX,
(FCL_REAL) rand() / RAND_MAX);
sample [i].extent2 = fcl::Vec3f ((FCL_REAL) rand() / RAND_MAX,
(FCL_REAL) rand() / RAND_MAX,
(FCL_REAL) rand() / RAND_MAX);
}
// Compute result for each function
Result resultDistance [nbSamples];
Result resultDisjoint [nbSamples];
Result resultDisjointAndLowerBound [nbSamples];
fcl::Transform3f tf1;
fcl::Transform3f tf2;
fcl::Box box1 (0, 0, 0);
fcl::Box box2 (0, 0, 0);
fcl::DistanceRequest request (false, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult result;
FCL_REAL distance;
FCL_REAL squaredDistance;
timeval t0, t1;
fcl::GJKSolver_indep gjkSolver;
// ShapeShapeDistance
gettimeofday (&t0, NULL);
for (std::size_t i=0; i<nbSamples; ++i) {
box1.side = 2*sample [i].extent1;
box2.side = 2*sample [i].extent2;
tf2.setTransform (sample [i].R, sample [i].T);
resultDistance [i].distance =
fcl::ShapeShapeDistance<fcl::Box, fcl::Box, fcl::GJKSolver_indep>
(&box1, tf1, &box2, tf2, &gjkSolver, request, result);
resultDistance [i].overlap = (resultDistance [i].distance < 0);
if (resultDistance [i].distance < 0) {
resultDistance [i].distance = 0;
}
result.clear ();
}
gettimeofday (&t1, NULL);
double t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
std::cout << "Time for " << nbSamples
<< " calls to ShapeShapeDistance (in seconds): "
<< t << std::endl;
// obbDisjointAndLowerBoundDistance
gettimeofday (&t0, NULL);
for (std::size_t i=0; i<nbSamples; ++i) {
resultDisjointAndLowerBound [i].overlap =
!obbDisjointAndLowerBoundDistance (sample [i].R, sample [i].T,
sample [i].extent1,
sample [i].extent2,
squaredDistance);
resultDisjointAndLowerBound [i].distance = sqrt (squaredDistance);
}
gettimeofday (&t1, NULL);
t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
std::cout << "Time for " << nbSamples
<< " calls to obbDisjointAndLowerBoundDistance"
<< " (in seconds): "
<< t << std::endl;
gettimeofday (&t0, NULL);
for (std::size_t i=0; i<nbSamples; ++i) {
resultDisjoint [i].overlap =
!obbDisjoint (sample [i].R, sample [i].T,
sample [i].extent1,
sample [i].extent2);
resultDisjoint [i].distance = 0;
}
gettimeofday (&t1, NULL);
t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
std::cout << "Time for " << nbSamples << " calls to obbDisjoint"
<< " (in seconds): "
<< t << std::endl;
// Test correctness of results
bool res1, res2, res3;
for (std::size_t i=0; i<nbSamples; ++i) {
res1 = resultDisjoint [i].overlap;
res2 = resultDisjointAndLowerBound [i].overlap;
res3 = resultDistance [i].overlap;