Commit e14ba8fc authored by Karsten1987's avatar Karsten1987 Committed by Florent Lamiraux
Browse files

extending capsule-capsule distance computation

  adding a test.
parent dd338233
......@@ -373,6 +373,11 @@ 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)
......@@ -838,6 +843,13 @@ bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Tra
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;
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
......
......@@ -46,6 +46,132 @@ namespace fcl
namespace details
{
// Clamp n to lie within the range [min, max]
float clamp(float n, float min, float max) {
if (n < min) return min;
if (n > max) return max;
return n;
}
// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
// distance between between S1(s) and S2(t)
float closestPtSegmentSegment(Vec3f p1, Vec3f q1, Vec3f p2, Vec3f q2,
float &s, float &t, Vec3f &c1, Vec3f &c2)
{
const float EPSILON = 0.001;
Vec3f d1 = q1 - p1; // Direction vector of segment S1
Vec3f d2 = q2 - p2; // Direction vector of segment S2
Vec3f r = p1 - p2;
float a = d1.dot(d1); // Squared length of segment S1, always nonnegative
float e = d2.dot(d2); // Squared length of segment S2, always nonnegative
float f = d2.dot(r);
// Check if either or both segments degenerate into points
if (a <= EPSILON && e <= EPSILON) {
// Both segments degenerate into points
s = t = 0.0f;
c1 = p1;
c2 = p2;
Vec3f diff = c1-c2;
float res = diff.dot(diff);
return res;
}
if (a <= EPSILON) {
// First segment degenerates into a point
s = 0.0f;
t = f / e; // s = 0 => t = (b*s + f) / e = f / e
t = clamp(t, 0.0f, 1.0f);
} else {
float c = d1.dot(r);
if (e <= EPSILON) {
// Second segment degenerates into a point
t = 0.0f;
s = clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a
} else {
// The general nondegenerate case starts here
float b = d1.dot(d2);
float denom = a*e-b*b; // Always nonnegative
// If segments not parallel, compute closest point on L1 to L2 and
// clamp to segment S1. Else pick arbitrary s (here 0)
if (denom != 0.0f) {
std::cerr << "demoninator equals zero, using 0 as reference" << std::endl;
s = clamp((b*f - c*e) / denom, 0.0f, 1.0f);
} else s = 0.0f;
// Compute point on L2 closest to S1(s) using
// t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
t = (b*s + f) / e;
//
//If t in [0,1] done. Else clamp t, recompute s for the new value
//of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
//and clamp s to [0, 1]
if(t < 0.0f) {
t = 0.0f;
s = clamp(-c / a, 0.0f, 1.0f);
} else if (t > 1.0f) {
t = 1.0f;
s = clamp((b - c) / a, 0.0f, 1.0f);
}
}
}
c1 = p1 + d1 * s;
c2 = p2 + d2 * t;
Vec3f diff = c1-c2;
float res = diff.dot(diff);
return res;
}
// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
// distance between between S1(s) and S2(t)
bool capsuleCapsuleDistance(const Capsule& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1_res, Vec3f* p2_res)
{
Vec3f p1(tf1.getTranslation());
Vec3f p2(tf2.getTranslation());
// line segment composes two points. First point is given by the origin, second point is computed by the origin transformed along z.
// extension along z-axis means transformation with identity matrix and translation vector z pos
Transform3f transformQ1(Vec3f(0,0,s1.lz));
transformQ1 = tf1 * transformQ1;
Vec3f q1 = transformQ1.getTranslation();
Transform3f transformQ2(Vec3f(0,0,s2.lz));
transformQ2 = tf2 * transformQ2;
Vec3f q2 = transformQ2.getTranslation();
// s and t correspont to the length of the line segment
float s, t;
Vec3f c1, c2;
float result = closestPtSegmentSegment(p1, q1, p2, q2, s, t, c1, c2);
*dist = sqrt(result)-s1.radius-s2.radius;
// getting directional unit vector
Vec3f distVec = c2 -c1;
distVec.normalize();
// extend the point to be border of the capsule.
// Done by following the directional unit vector for the length of the capsule radius
*p1_res = c1 + distVec*s1.radius;
distVec = c1-c2;
distVec.normalize();
*p2_res = c2 + distVec*s2.radius;
return true;
}
// Compute the point on a line segment that is the closest point on the
// segment to to another point. The code is inspired by the explanation
// given by Dan Sunday's page:
......@@ -2858,8 +2984,20 @@ bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Trans
}
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
{
return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
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
{
return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
} // fcl
......@@ -28,6 +28,7 @@ add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp)
add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
#add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
......
......@@ -1862,12 +1862,12 @@ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
Sphere s2(10);
Transform3f transform;
generateRandomTransform(extents, transform);
//generateRandomTransform(extents, transform);
bool res;
FCL_REAL dist = -1;
Vec3f closest_p1, closest_p2;
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 40, 0)), &dist, &closest_p1, &closest_p2);
BOOST_CHECK(fabs(dist - 10) < 0.001);
BOOST_CHECK(res);
......@@ -1922,9 +1922,10 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
{
Box s1(20, 40, 50);
Box s2(10, 10, 10);
Vec3f closest_p1, closest_p2;
Transform3f transform;
generateRandomTransform(extents, transform);
//generateRandomTransform(extents, transform);
bool res;
FCL_REAL dist;
......@@ -1937,10 +1938,44 @@ BOOST_AUTO_TEST_CASE(shapeDistance_boxbox)
BOOST_CHECK(dist < 0);
BOOST_CHECK_FALSE(res);
res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist);
std::cerr << " SOVLER NUMBER 1" << std::endl;
res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2);
std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2);
std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)), &dist, &closest_p1, &closest_p2);
std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2);
std::cerr << "computed points in box to box" << closest_p1 << " & " << closest_p2 << "with dist: " << dist<< std::endl;
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)), &dist, &closest_p1, &closest_p2);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
......@@ -2719,40 +2754,6 @@ BOOST_AUTO_TEST_CASE(conecone)
BOOST_CHECK(res);
}
BOOST_AUTO_TEST_CASE(conecylinder)
{
Cylinder s1(5, 10);
Cone s2(5, 10);
Transform3f transform;
generateRandomTransform(extents, transform);
bool res;
FCL_REAL dist;
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
BOOST_CHECK(dist < 0);
BOOST_CHECK_FALSE(res);
res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
BOOST_CHECK(dist < 0);
BOOST_CHECK_FALSE(res);
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 0.1) < 0.001);
BOOST_CHECK(res);
res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 30) < 0.001);
BOOST_CHECK(res);
res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
BOOST_CHECK(fabs(dist - 30) < 0.001);
BOOST_CHECK(res);
}
......@@ -167,6 +167,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_collision)
FCL_REAL distance;
BOOST_CHECK (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated)
......@@ -181,8 +182,33 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated)
Transform3f capsule_transform (Vec3f (0., 0., 175));
FCL_REAL distance = 0.;
Vec3f p1;
Vec3f p2;
bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance);
BOOST_CHECK (is_separated);
BOOST_CHECK (distance == 25.);
}
BOOST_AUTO_TEST_CASE(Capsule_Capsule_Distance_test_separated)
{
GJKSolver_libccd solver;
Capsule sphere1 (50, 0);
Transform3f sphere1_transform(Vec3f (0., 0., 0));
Capsule capsule (50, 00.);
Transform3f capsule_transform (Vec3f (150., 0., 0));
FCL_REAL distance = 0.;
Vec3f p1;
Vec3f p2;
bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance, &p1, &p2);
std::cerr << "computed distance: " << distance << std::endl;
std::cerr << "computed p1: " << p1 << std::endl;
std::cerr << "computed p2: " << p2 << std::endl;
BOOST_CHECK (is_separated);
BOOST_CHECK (distance == 25.);
......
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