Commit 96c47efb authored by rstrudel's avatar rstrudel
Browse files

Take into account jmirabel PR comments

parent 96254626
...@@ -52,14 +52,14 @@ namespace fcl { ...@@ -52,14 +52,14 @@ namespace fcl {
FCL_REAL radius2 = capsule2->radius; FCL_REAL radius2 = capsule2->radius;
// direction of capsules // direction of capsules
// ||d1|| = 2 * halfLength1 // ||d1|| = 2 * halfLength1
const fcl::Vec3f& d1 = 2 * halfLength1 * tf1.getRotation().col(2); const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2);
const fcl::Vec3f& d2 = 2 * halfLength2 * tf2.getRotation().col(2); const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2);
// Starting point of the segments // Starting point of the segments
// p1 + d1 is the end point of the segment // p1 + d1 is the end point of the segment
const fcl::Vec3f& p1 = c1 - d1 / 2; const fcl::Vec3f p1 = c1 - d1 / 2;
const fcl::Vec3f& p2 = c2 - d2 / 2; const fcl::Vec3f p2 = c2 - d2 / 2;
const fcl::Vec3f& r = p1-p2; const fcl::Vec3f r = p1-p2;
FCL_REAL a = d1.dot(d1); FCL_REAL a = d1.dot(d1);
FCL_REAL b = d1.dot(d2); FCL_REAL b = d1.dot(d2);
FCL_REAL c = d1.dot(r); FCL_REAL c = d1.dot(r);
...@@ -76,6 +76,7 @@ namespace fcl { ...@@ -76,6 +76,7 @@ namespace fcl {
s = t = 0.0; s = t = 0.0;
FCL_REAL distance = (p1-p2).norm(); FCL_REAL distance = (p1-p2).norm();
Vec3f normal = (p1 - p2) / distance; Vec3f normal = (p1 - p2) / distance;
result.normal = normal;
distance = distance - (radius1 + radius2); distance = distance - (radius1 + radius2);
result.min_distance = distance; result.min_distance = distance;
if (request.enable_nearest_points) if (request.enable_nearest_points)
...@@ -100,9 +101,9 @@ namespace fcl { ...@@ -100,9 +101,9 @@ namespace fcl {
else else
{ {
// Always non-negative, equal 0 if the segments are parallel // Always non-negative, equal 0 if the segments are parallel
FCL_REAL denom = a*e-b*b; FCL_REAL denom = fmax(a*e-b*b, 0);
if (denom != 0.0) if (denom > EPSILON)
{ {
s = CLAMP((b*f-c*e) / denom, 0.0, 1.0); s = CLAMP((b*f-c*e) / denom, 0.0, 1.0);
} }
...@@ -129,6 +130,7 @@ namespace fcl { ...@@ -129,6 +130,7 @@ namespace fcl {
const Vec3f& w2 = p2 + t * d2; const Vec3f& w2 = p2 + t * d2;
FCL_REAL distance = (w1 - w2).norm(); FCL_REAL distance = (w1 - w2).norm();
Vec3f normal = (w1 - w2) / distance; Vec3f normal = (w1 - w2) / distance;
result.normal = normal;
// capsule spcecific distance computation // capsule spcecific distance computation
distance = distance - (radius1 + radius2); distance = distance - (radius1 + radius2);
...@@ -166,7 +168,7 @@ namespace fcl { ...@@ -166,7 +168,7 @@ namespace fcl {
const Vec3f& p1 = distanceResult.nearest_points [0]; const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1]; const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = 0.5 * (p1+p2); contact.pos = 0.5 * (p1+p2);
contact.normal = (p2-p1)/(p2-p1).norm (); contact.normal = distanceResult.normal;
result.addContact(contact); result.addContact(contact);
return 1; return 1;
} }
......
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