From 6133f32ab06cad94123fdcb566b9931576fb0942 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 1 Jul 2015 10:47:50 +0200
Subject: [PATCH] Express closest points in global frame
- The frame in which closest points were expressed was neither clear, nor
consistent.
---
include/hpp/fcl/narrowphase/narrowphase.h | 8 +++---
src/distance.cpp | 9 +++++++
src/distance_capsule_capsule.cpp | 9 +++----
src/narrowphase/narrowphase.cpp | 32 +++++++++--------------
test/test_fcl_box_box_distance.cpp | 25 +++++++++---------
test/test_fcl_capsule_box_1.cpp | 16 ++++++------
test/test_fcl_capsule_box_2.cpp | 12 ++++-----
7 files changed, 57 insertions(+), 54 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index a09c3ddc..0db42cdf 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -617,8 +617,8 @@ struct GJKSolver_indep
if(distance) *distance = (w0 - w1).length();
- if(p1) *p1 = w0;
- if(p2) *p2 = w1;
+ if(p1) *p1 = tf1.transform (w0);
+ if(p2) *p2 = tf1.transform (w1);
return true;
}
@@ -718,8 +718,8 @@ struct GJKSolver_indep
}
if(distance) *distance = (w0 - w1).length();
- if(p1) *p1 = w0;
- if(p2) *p2 = shape.toshape0.transform(w1);
+ if(p1) *p1 = tf1.transform(w0);
+ if(p2) *p2 = tf1.transform(w1);
return true;
}
else
diff --git a/src/distance.cpp b/src/distance.cpp
index 30341788..c86cdd5e 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -87,6 +87,15 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
else
{
res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result);
+ // If closest points are requested, switch object 1 and 2
+ if (request.enable_nearest_points) {
+ const CollisionGeometry *tmpo = result.o1;
+ result.o1 = result.o2;
+ result.o2 = tmpo;
+ Vec3f tmpn (result.nearest_points [0]);
+ result.nearest_points [0] = result.nearest_points [1];
+ result.nearest_points [1] = tmpn;
+ }
}
}
else
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 26380b79..7239066a 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -35,12 +35,11 @@ namespace fcl {
const Capsule* c2 = static_cast <const Capsule*> (o2);
// We assume that capsules are centered at the origin.
- fcl::Vec3f center1 = Vec3f (0, 0, 0);
- fcl::Vec3f center2 = inverse (tf1).transform (tf2.getTranslation ());
+ fcl::Vec3f center1 = tf1.getTranslation ();
+ fcl::Vec3f center2 = tf2.getTranslation ();
// We assume that capsules are oriented along z-axis.
- fcl::Vec3f direction1 = Vec3f (0, 0, 1.);
- fcl::Vec3f direction2 = tf1.getRotation ().transposeTimes
- (tf2.getRotation ().getColumn (2));
+ fcl::Vec3f direction1 = tf1.getRotation ().getColumn (2);
+ fcl::Vec3f direction2 = tf2.getRotation ().getColumn (2);
FCL_REAL halfLength1 = 0.5*c1->lz;
FCL_REAL halfLength2 = 0.5*c2->lz;
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 56c7014f..5aee9c7b 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -111,9 +111,9 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
Transform3f tf2_inv(tf2);
tf2_inv.inverse();
- Vec3f pos1(0., 0., 0.5 * s2.lz);
- Vec3f pos2(0., 0., -0.5 * s2.lz);
- Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f()));
+ Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz)));
+ Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
+ Vec3f s_c = tf1.getTranslation ();
Vec3f segment_point;
@@ -122,20 +122,19 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
FCL_REAL distance = diff.length() - s1.radius - s2.radius;
- if(distance <= 0)
- return false;
-
if(dist) *dist = distance;
if(p1 || p2) diff.normalize();
if(p1)
{
*p1 = s_c - diff * s1.radius;
- *p1 = inverse(tf1).transform(tf2.transform(*p1));
}
if(p2) *p2 = segment_point + diff * s1.radius;
+ if(distance <= 0)
+ return false;
+
return true;
}
@@ -176,16 +175,13 @@ bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
Vec3f o2 = tf2.getTranslation();
Vec3f diff = o1 - o2;
FCL_REAL len = diff.length();
- if(len > s1.radius + s2.radius)
- {
- if(dist) *dist = len - (s1.radius + s2.radius);
- if(p1) *p1 = inverse(tf1).transform(o1 - diff * (s1.radius / len));
- if(p2) *p2 = inverse(tf2).transform(o2 + diff * (s2.radius / len));
- return true;
- }
+ FCL_REAL d (len > s1.radius + s2.radius);
- if(dist) *dist = -1;
- return false;
+ if(dist) *dist = len - (s1.radius + s2.radius);
+ if(p1) *p1 = o1 - diff * (s1.radius / len);
+ if(p2) *p2 = o2 + diff * (s2.radius / len);
+
+ return (d >=0);
}
/** \brief the minimum distance from a point to a line */
@@ -584,7 +580,7 @@ bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
Vec3f dir = o - project_p;
dir.normalize();
- if(p1) { *p1 = o - dir * sp.radius; *p1 = inverse(tf).transform(*p1); }
+ if(p1) { *p1 = o - dir * sp.radius; }
if(p2) *p2 = project_p;
return true;
}
@@ -603,8 +599,6 @@ bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
{
bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2);
- if(p2) *p2 = inverse(tf2).transform(*p2);
-
return res;
}
diff --git a/test/test_fcl_box_box_distance.cpp b/test/test_fcl_box_box_distance.cpp
index fdae2a49..5fd1bea4 100644
--- a/test/test_fcl_box_box_distance.cpp
+++ b/test/test_fcl_box_box_distance.cpp
@@ -87,7 +87,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_1)
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
- BOOST_CHECK_CLOSE(distanceResult.min_distance,
+ BOOST_CHECK_CLOSE(distanceResult.min_distance,
sqrt (dx*dx + dy*dy + dz*dz), 1e-4);
BOOST_CHECK_CLOSE (p1 [0], 3, 1e-6);
@@ -133,7 +133,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2)
BOOST_CHECK_CLOSE (p1 [0], 0.60947571, 1e-4);
BOOST_CHECK_CLOSE (p1 [1], 0.01175873, 1e-4);
- BOOST_CHECK_CLOSE (p1 [2], 1., 1e-6);
+ BOOST_CHECK_CLOSE (p1 [2], 1, 1e-6);
BOOST_CHECK_CLOSE (p2 [0], 0.60947571, 1e-4);
BOOST_CHECK_CLOSE (p2 [1], 0.01175873, 1e-4);
BOOST_CHECK_CLOSE (p2 [2], -1.62123444 + 10, 1e-4);
@@ -172,9 +172,8 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
double distance = 4 - sqrt (2);
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
- const Vec3f p1Ref (.5, -.5, 0);
- const Vec3f p2Ref (2.3284271247461898, -2.3284271247461898, 0);
-
+ const Vec3f p1Ref (sqrt(2)/2 - 2, 1, .5);
+ const Vec3f p2Ref (2 - sqrt(2)/2, 1, .5);
BOOST_CHECK_CLOSE (p1 [0], p1Ref [0], 1e-4);
BOOST_CHECK_CLOSE (p1 [1], p1Ref [1], 1e-4);
BOOST_CHECK_CLOSE (p1 [2], p1Ref [2], 1e-4);
@@ -205,11 +204,13 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
- BOOST_CHECK_CLOSE (p1 [0], p1Ref [0], 1e-4);
- BOOST_CHECK_CLOSE (p1 [1], p1Ref [1], 1e-4);
- CHECK_CLOSE_TO_0 (p1 [2], 1e-6);
- BOOST_CHECK_CLOSE (p2 [0], p2Ref [0], 1e-4);
- BOOST_CHECK_CLOSE (p2 [1], p2Ref [1], 1e-4);
- CHECK_CLOSE_TO_0 (p2 [2], 1e-6);
-
+ const Vec3f p1Moved = tf3.transform (p1Ref);
+ const Vec3f p2Moved = tf3.transform (p2Ref);
+ BOOST_CHECK_CLOSE (p1 [0], p1Moved [0], 1e-4);
+ BOOST_CHECK_CLOSE (p1 [1], p1Moved [1], 1e-4);
+ BOOST_CHECK_CLOSE (p1 [2], p1Moved [2], 1e-4);
+ BOOST_CHECK_CLOSE (p2 [0], p2Moved [0], 1e-4);
+ BOOST_CHECK_CLOSE (p2 [1], p2Moved [1], 1e-4);
+ BOOST_CHECK_CLOSE (p2 [2], p2Moved [2], 1e-4);
+
}
diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp
index 938d2047..16159ed1 100644
--- a/test/test_fcl_capsule_box_1.cpp
+++ b/test/test_fcl_capsule_box_1.cpp
@@ -70,9 +70,9 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
// Nearest point on box
fcl::Vec3f o2 (distanceResult.nearest_points [1]);
BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-1);
- BOOST_CHECK_CLOSE (o1 [0], -2.0, 1e-4);
+ BOOST_CHECK_CLOSE (o1 [0], 1.0, 1e-1);
CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
- BOOST_CHECK_CLOSE (o2 [0], -2.5, 1e-4);
+ BOOST_CHECK_CLOSE (o2 [0], 0.5, 1e-1);
CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
// Move capsule above box
@@ -88,11 +88,11 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
BOOST_CHECK_CLOSE (distanceResult.min_distance, 2.0, 1e-1);
CHECK_CLOSE_TO_0 (o1 [0], 1e-1);
CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
- BOOST_CHECK_CLOSE (o1 [2], -4.0, 1e-4);
+ BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-1);
CHECK_CLOSE_TO_0 (o2 [0], 1e-1);
CHECK_CLOSE_TO_0 (o2 [1], 1e-1);
- BOOST_CHECK_CLOSE (o2 [2], -6.0, 1e-4);
+ BOOST_CHECK_CLOSE (o2 [2], 2.0, 1e-1);
// Rotate capsule around y axis by pi/2 and move it behind box
tf1.setTranslation (fcl::Vec3f (-10., 0., 0.));
@@ -106,10 +106,10 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
o2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-1);
- CHECK_CLOSE_TO_0 (o1 [0], 1e-1);
+ BOOST_CHECK_CLOSE (o1 [0], -6, 1e-2);
CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
- BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-1);
- CHECK_CLOSE_TO_0 (o2 [0], 1e-1);
+ CHECK_CLOSE_TO_0 (o1 [2], 1e-1);
+ BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-2);
CHECK_CLOSE_TO_0 (o2 [1], 1e-1);
- BOOST_CHECK_CLOSE (o2 [2], 9.5, 1e-1);
+ CHECK_CLOSE_TO_0 (o2 [2], 1e-1);
}
diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp
index a612cefb..81e355bc 100644
--- a/test/test_fcl_capsule_box_2.cpp
+++ b/test/test_fcl_capsule_box_2.cpp
@@ -72,10 +72,10 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
fcl::Vec3f o2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-2);
- CHECK_CLOSE_TO_0 (o1 [0], 1e-2);
- CHECK_CLOSE_TO_0 (o1 [1], 1e-1);
- BOOST_CHECK_CLOSE (o1 [2], 4., 1e-2);
- CHECK_CLOSE_TO_0 (o2 [0], 1e-2);
- CHECK_CLOSE_TO_0 (o2 [1], 1e-1);
- BOOST_CHECK_CLOSE (o2 [2], 9.5, 1e-2);
+ BOOST_CHECK_CLOSE (o1 [0], -6, 1e-2);
+ BOOST_CHECK_CLOSE (o1 [1], 0.8, 1e-1);
+ BOOST_CHECK_CLOSE (o1 [2], 1.5, 1e-2);
+ BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-2);
+ BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-1);
+ BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-2);
}
--
GitLab