Skip to content

GitLab

Commit 6a1930cd authored by rstrudel's avatar rstrudel
Browse files

Rewrite Capsule/Capsule distance function

parent 126cd3d7
Hide whitespace changes
Inline Side-by-side
src/distance_capsule_capsule.cpp
View file @ 6a1930cd
......@@ -12,6 +12,8 @@
#include <hpp/fcl/shape/geometric_shapes.h>
#include <../src/distance_func_matrix.h>
#define CLAMP(value, l, u) fmax(fmin(value, u), l)
// Note that partial specialization of template functions is not allowed.
// Therefore, two implementations with the default narrow phase solvers are
// provided. If another narrow phase solver were to be used, the default
......@@ -25,299 +27,121 @@ namespace hpp
namespace fcl {
class GJKSolver;
// Compute the distance between C1 and C2 by computing the distance
// between the two segments supporting the capsules.
// Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest Point of Two Line Segments
template <>
FCL_REAL ShapeShapeDistance <Capsule, Capsule>
(const CollisionGeometry* o1, const Transform3f& tf1,
FCL_REAL ShapeShapeDistance <Capsule, Capsule> (const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest& request,
DistanceResult& result)
{
const Capsule* c1 = static_cast <const Capsule*> (o1);
const Capsule* c2 = static_cast <const Capsule*> (o2);
const Capsule* capsule1 = static_cast <const Capsule*> (o1);
const Capsule* capsule2 = static_cast <const Capsule*> (o2);
FCL_REAL EPSILON = std::numeric_limits<FCL_REAL>::epsilon () * 100;
// We assume that capsules are centered at the origin.
const fcl::Vec3f& center1 = tf1.getTranslation ();
const fcl::Vec3f& center2 = tf2.getTranslation ();
const fcl::Vec3f& c1 = tf1.getTranslation ();
const fcl::Vec3f& c2 = tf2.getTranslation ();
// We assume that capsules are oriented along z-axis.
Matrix3f::ConstColXpr direction1 = tf1.getRotation ().col (2);
Matrix3f::ConstColXpr direction2 = tf2.getRotation ().col (2);
FCL_REAL halfLength1 = c1->halfLength;
FCL_REAL halfLength2 = c2->halfLength;
FCL_REAL halfLength1 = capsule1->halfLength;
FCL_REAL halfLength2 = capsule2->halfLength;
FCL_REAL radius1 = capsule1->radius;
FCL_REAL radius2 = capsule2->radius;
// direction of capsules
// ||d1|| = 2 * halfLength1
// Matrix3f::ConstColXpr d1 = 2 * halfLength1 * tf1.getRotation().col(2);
// Matrix3f::ConstColXpr d2 = 2 * halfLength2 * tf2.getRotation().col(2);
const fcl::Vec3f& d1 = 2 * halfLength1 * tf1.getRotation().col(2);
const fcl::Vec3f& d2 = 2 * halfLength2 * tf2.getRotation().col(2);
Vec3f diff = center1 - center2;
FCL_REAL a01 = -direction1.dot (direction2);
FCL_REAL b0 = diff.dot (direction1);
FCL_REAL b1 = -diff.dot (direction2);
FCL_REAL c = diff.squaredNorm ();
FCL_REAL det = fabs (1.0 - a01*a01);
FCL_REAL s1, s2, sqrDist, extDet0, extDet1, tmpS0, tmpS1;
FCL_REAL epsilon = std::numeric_limits<FCL_REAL>::epsilon () * 100;
// Starting point of the segments
// p1 + d1 is the end point of the segment
const fcl::Vec3f& p1 = c1 - d1 / 2;
const fcl::Vec3f& p2 = c2 - d2 / 2;
const fcl::Vec3f& r = p1-p2;
FCL_REAL a = d1.dot(d1);
FCL_REAL b = d1.dot(d2);
FCL_REAL c = d1.dot(r);
FCL_REAL e = d2.dot(d2);
FCL_REAL f = d2.dot(r);
// S1 is parametrized by the equation p1 + s * d1
// S2 is parametrized by the equation p2 + t * d2
FCL_REAL s = 0.0;
FCL_REAL t = 0.0;
if (det >= epsilon) {
// Segments are not parallel.
s1 = a01*b1 - b0;
s2 = a01*b0 - b1;
extDet0 = halfLength1*det;
extDet1 = halfLength2*det;
if (s1 >= -extDet0) {
if (s1 <= extDet0) {
if (s2 >= -extDet1) {
if (s2 <= extDet1) { // region 0 (interior)
// Minimum at interior points of segments.
FCL_REAL invDet = (1.0)/det;
s1 *= invDet;
s2 *= invDet;
sqrDist = s1*(s1 + a01*s2 + 2.0*b0) +
s2*(a01*s1 + s2 + 2.0*b1) + c;
}
else { // region 3 (side)
s2 = halfLength2;
tmpS0 = -(a01*s2 + b0);
if (tmpS0 < -halfLength1) {
s1 = -halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
}
else if (tmpS0 <= halfLength1) {
s1 = tmpS0;
sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
}
else {
s1 = halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
}
}
}
else { // region 7 (side)
s2 = -halfLength2;
tmpS0 = -(a01*s2 + b0);
if (tmpS0 < -halfLength1) {
s1 = -halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
} else if (tmpS0 <= halfLength1) {
s1 = tmpS0;
sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
} else {
s1 = halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
}
}
}
else {
if (s2 >= -extDet1) {
if (s2 <= extDet1) { // region 1 (side)
s1 = halfLength1;
tmpS1 = -(a01*s1 + b1);
if (tmpS1 < -halfLength2) {
s2 = -halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
else if (tmpS1 <= halfLength2) {
s2 = tmpS1;
sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
}
else {
s2 = halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
}
else { // region 2 (corner)
s2 = halfLength2;
tmpS0 = -(a01*s2 + b0);
if (tmpS0 < -halfLength1) {
s1 = -halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
}
else if (tmpS0 <= halfLength1) {
s1 = tmpS0;
sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
}
else {
s1 = halfLength1;
tmpS1 = -(a01*s1 + b1);
if (tmpS1 < -halfLength2) {
s2 = -halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
else if (tmpS1 <= halfLength2) {
s2 = tmpS1;
sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
}
else {
s2 = halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
}
}
}
else { // region 8 (corner)
s2 = -halfLength2;
tmpS0 = -(a01*s2 + b0);
if (tmpS0 < -halfLength1) {
s1 = -halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
}
else if (tmpS0 <= halfLength1) {
s1 = tmpS0;
sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
}
else {
s1 = halfLength1;
tmpS1 = -(a01*s1 + b1);
if (tmpS1 > halfLength2) {
s2 = halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
else if (tmpS1 >= -halfLength2) {
s2 = tmpS1;
sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
}
else {
s2 = -halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
}
}
}
}
else {
if (s2 >= -extDet1) {
if (s2 <= extDet1) { // region 5 (side)
s1 = -halfLength1;
tmpS1 = -(a01*s1 + b1);
if (tmpS1 < -halfLength2) {
s2 = -halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
else if (tmpS1 <= halfLength2) {
s2 = tmpS1;
sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
}
else {
s2 = halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
}
else { // region 4 (corner)
s2 = halfLength2;
tmpS0 = -(a01*s2 + b0);
if (tmpS0 > halfLength1) {
s1 = halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
}
else if (tmpS0 >= -halfLength1) {
s1 = tmpS0;
sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
}
else {
s1 = -halfLength1;
tmpS1 = -(a01*s1 + b1);
if (tmpS1 < -halfLength2) {
s2 = -halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
else if (tmpS1 <= halfLength2) {
s2 = tmpS1;
sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
}
else {
s2 = halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
}
}
}
else { // region 6 (corner)
s2 = -halfLength2;
tmpS0 = -(a01*s2 + b0);
if (tmpS0 > halfLength1) {
s1 = halfLength1;
sqrDist = s1*(s1 - 2.0*tmpS0) +
s2*(s2 + 2.0*b1) + c;
}
else if (tmpS0 >= -halfLength1) {
s1 = tmpS0;
sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
}
else {
s1 = -halfLength1;
tmpS1 = -(a01*s1 + b1);
if (tmpS1 < -halfLength2) {
s2 = -halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
else if (tmpS1 <= halfLength2) {
s2 = tmpS1;
sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
}
else {
s2 = halfLength2;
sqrDist = s2*(s2 - 2.0*tmpS1) +
s1*(s1 + 2.0*b0) + c;
}
}
}
if (a <= EPSILON && e <= EPSILON)
{
// Check if the segments degenerate into points
s = t = 0.0;
FCL_REAL distance = (p1-p2).norm();
Vec3f normal = (p1 - p2) / distance;
distance = distance - (radius1 + radius2);
result.min_distance = distance;
if (request.enable_nearest_points)
{
result.nearest_points[0] = p1 - radius1 * normal;
result.nearest_points[1] = p2 + radius2 * normal;
}
return distance;
}
else if (a <= EPSILON)
{
// First segment is degenerated
s = 0.0;
t = CLAMP(f / e, 0.0, 1.0);
}
else {
// The segments are parallel. The average b0 term is designed to
// ensure symmetry of the function. That is, dist(seg0,seg1) and
// dist(seg1,seg0) should produce the same number.
FCL_REAL e0pe1 = halfLength1 + halfLength2;
FCL_REAL sign = (a01 > 0.0 ? -1.0 : 1.0);
FCL_REAL b0Avr = (0.5)*(b0 - sign*b1);
FCL_REAL lambda = -b0Avr;
if (lambda < -e0pe1) {
lambda = -e0pe1;
else if (e <= EPSILON)
{
// Second segment is degenerated
s = CLAMP(-c / a, 0.0, 1.0);
t = 0.0;
}
else
{
// Always non-negative, equal 0 if the segments are parallel
FCL_REAL denom = a*e-b*b;
if (denom != 0.0)
{
s = CLAMP((b*f-c*e) / denom, 0.0, 1.0);
}
else if (lambda > e0pe1) {
lambda = e0pe1;
else
{
s = 0.0;
}
s2 = -sign*lambda*halfLength2/e0pe1;
s1 = lambda + sign*s2;
sqrDist = lambda*(lambda + 2.0*b0Avr) + c;
t = (b*s + f) / e;
if (t < 0.0)
{
t = 0.0;
s = CLAMP(-c / a, 0.0, 1.0);
}
else if (t > 1.0)
{
t = 1.0;
s = CLAMP((b - c)/a, 0.0, 1.0);
}
}
Vec3f closestOnSegment1 = center1 + s1*direction1;
Vec3f closestOnSegment2 = center2 + s2*direction2;
// witness points achieving the distance between the two segments
const Vec3f& w1 = p1 + s * d1;
const Vec3f& w2 = p2 + t * d2;
FCL_REAL distance = (w1 - w2).norm();
Vec3f normal = (w1 - w2) / distance;
Vec3f unitVector = closestOnSegment2 - closestOnSegment1;
FCL_REAL distance = sqrt (sqrDist);
if (distance >= epsilon) {
unitVector /= distance;
} else {
unitVector.setZero ();
// capsule spcecific distance computation
distance = distance - (radius1 + radius2);
result.min_distance = distance;
// witness points for the capsules
if (request.enable_nearest_points)
{
result.nearest_points[0] = w1 - radius1 * normal;
result.nearest_points[1] = w2 + radius2 * normal;
}
// Update distance result.
result.min_distance = distance - c1->radius - c2->radius;
if (request.enable_nearest_points) {
result.nearest_points [0] = closestOnSegment1 + c1->radius * unitVector;
result.nearest_points [1] = closestOnSegment2 - c2->radius * unitVector;
}
result.o1 = o1;
result.o2 = o2;
result.b1 = -1;
result.b2 = -1;
return result.min_distance;
return distance;
}
template <>
......@@ -334,18 +158,22 @@ namespace fcl {
FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule>
(o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
if (distance <= 0) {
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
if (distance > 0)
{
return 0;
}
else
{
Contact contact (o1, o2, -1, -1);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
contact.pos = 0.5 * (p1+p2);
contact.normal = (p2-p1)/(p2-p1).norm ();
result.addContact (contact);
result.addContact(contact);
return 1;
}
result.distance_lower_bound = distance;
return 0;
}
} // namespace fcl
} // namespace hpp
test/capsule_capsule.cpp
View file @ 6a1930cd
......@@ -308,13 +308,14 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
distance (&o1, &o2, distanceRequest, distanceResult);
std::cerr << "Applied rotation and translation on two capsules" << std::endl;
std::cerr << "R1 = " << tf1.getRotation ()
<< ", T1 = " << tf1.getTranslation() << std::endl
<< "R2 = " << tf2.getRotation ()
<< ", T2 = " << tf2.getTranslation() << std::endl;
std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
<< ", p2 = " << distanceResult.nearest_points [1]
<< ", distance = " << distanceResult.min_distance << std::endl;
std::cerr << "R1 = " << tf1.getRotation () << std::endl
<< "T1 = " << tf1.getTranslation().transpose() << std::endl
<< "R2 = " << tf2.getRotation () << std::endl
<< "T2 = " << tf2.getTranslation().transpose() << std::endl;
std::cerr << "Closest points:" << std::endl
<< "p1 = " << distanceResult.nearest_points[0].transpose() << std::endl
<< "p2 = " << distanceResult.nearest_points[1].transpose() << std::endl
<< "distance = " << distanceResult.min_distance << std::endl;
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
......
Markdown is supported
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