diff --git a/include/fcl/collision_data.h b/include/fcl/collision_data.h
index e299e26ede247732092ac9992c3fe62d6b747d83..eee6acf4ebe89fcbd44f0127d249a98816801a9a 100644
--- a/include/fcl/collision_data.h
+++ b/include/fcl/collision_data.h
@@ -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()
{
diff --git a/include/fcl/math/matrix_3f.h b/include/fcl/math/matrix_3f.h
index 9ecbeee5fbb2b63414a616e9a4a116b8f141c6d8..9bfec2b37d81e430dfc5c705ffbfb0628e3cf7c0 100644
--- a/include/fcl/math/matrix_3f.h
+++ b/include/fcl/math/matrix_3f.h
@@ -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);
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 60f9de15705c87d264b306e2249c4e30f3ad07e0..f54fe602b076d6654a988f9c59a994b834177881 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -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
diff --git a/src/BV/OBB.h b/src/BV/OBB.h
new file mode 100644
index 0000000000000000000000000000000000000000..c00b7793468c9f6a9bd644f2a0a19777474c7171
--- /dev/null
+++ b/src/BV/OBB.h
@@ -0,0 +1,50 @@
+/*
+ * 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
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 118bf11f66a145f3087b0ed06c7c53e11e34de5e..fc49ea3bdf552e5cc106792dd8aea62ef6443d38 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -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]));
diff --git a/src/collision.cpp b/src/collision.cpp
index b3569e4dc92d4705cb9bca9733badd3a591a4473..5979fe8d92c60adb55b37f3fcd76effbefe1816e 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -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)
{
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 32ce18e7bbea2872e39a340fa84b012798ad91c1..40ca30366359f71f5e34c4fb32bc8d97623f5ed3 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -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);
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 112a1ff6cef93ebae20a8507c15d61a55b5adea1..7b88f61aafdb1f7419aa96e808ffe494da27dba4 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -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;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 77d81bf2810a68cdabac38355c167632fd02c8fe..66005fcb477c61ade405d7313fd213cd5d64d11b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -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)
diff --git a/test/test_fcl_obb.cpp b/test/test_fcl_obb.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42d173e8feb8c749a71a0971ea0ca56987f04b2c
--- /dev/null
+++ b/test/test_fcl_obb.cpp
@@ -0,0 +1,194 @@
+/*
+ * 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;
+ sumDistanceLowerBound += resultDisjointAndLowerBound [i].distance;
+ sumDistance += resultDistance [i].distance;
+
+#if 0
+ std::cout << "ShapeShapeDistance: overlap: "
+ << resultDistance [i].overlap
+ << ", distance: " << resultDistance [i].distance << std::endl;
+ std::cout << "disjoint: overlap: "
+ << resultDisjoint [i].overlap
+ << ", distance: " << resultDisjoint [i].distance << std::endl;
+ std::cout << "disjointAndLowerBound: overlap: "
+ << resultDisjointAndLowerBound [i].overlap
+ << ", distance: " << resultDisjointAndLowerBound [i].distance
+ << std::endl << std::endl;
+#endif
+ BOOST_CHECK (res1 == res2);
+ BOOST_CHECK (res1 == res3);
+ BOOST_CHECK ((!res1 && resultDisjointAndLowerBound [i].distance > 0) ||
+ (res1 && resultDisjointAndLowerBound [i].distance == 0));
+ BOOST_CHECK (resultDisjointAndLowerBound [i].distance <=
+ resultDistance [i].distance);
+ }
+ std::cout << "Sum distances ShapeShapeDistance: "
+ << sumDistance << std::endl;
+ std::cout << "Sum distances obbDisjointAndLowerBoundDistance: "
+ << sumDistanceLowerBound << std::endl;
+}