Merge pull request #110 from sherm1/fix-bug-in-propagateBVHFrontListCollisionRecurse

Fixes a nasty bug in propagateBVHFrontListCollisionRecurse()

Author: sherm1

src/ccd/motion.cpp

@@ -54,12 +54,12 @@ FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const SplineMotion& motion) const
 
   FCL_REAL tmp;
   // max_i |c_i * n|
-  FCL_REAL cn_max = std::fabs(c1.dot(n));
-  tmp = std::fabs(c2.dot(n));
+  FCL_REAL cn_max = std::abs(c1.dot(n));
+  tmp = std::abs(c2.dot(n));
   if(tmp > cn_max) cn_max = tmp;
-  tmp = std::fabs(c3.dot(n));
+  tmp = std::abs(c3.dot(n));
   if(tmp > cn_max) cn_max = tmp;
-  tmp = std::fabs(c4.dot(n));
+  tmp = std::abs(c4.dot(n));
   if(tmp > cn_max) cn_max = tmp;
 
   // max_i ||c_i||
@@ -99,10 +99,10 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const SplineMotion& motion) const
   FCL_REAL T_bound = motion.computeTBound(n);
   FCL_REAL tf_t = motion.getCurrentTime();
 
-  FCL_REAL R_bound = std::fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
-  FCL_REAL R_bound_tmp = std::fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
+  FCL_REAL R_bound = std::abs(a.dot(n)) + a.length() + (a.cross(n)).length();
+  FCL_REAL R_bound_tmp = std::abs(b.dot(n)) + b.length() + (b.cross(n)).length();
   if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
-  R_bound_tmp = std::fabs(c.dot(n)) + c.length() + (c.cross(n)).length();
+  R_bound_tmp = std::abs(c.dot(n)) + c.length() + (c.cross(n)).length();
   if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
 
   FCL_REAL dWdW_max = motion.computeDWMax();

src/narrowphase/narrowphase.cpp

@@ -95,7 +95,7 @@ namespace details
 	// 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;
+	  std::cerr << "denominator 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

src/traversal/traversal_recurse.cpp

@@ -414,7 +414,7 @@ void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVH
         if(node->firstOverSecond(b1, b2))
         {
           int c1 = node->getFirstLeftChild(b1);
-          int c2 = node->getFirstRightChild(b2);
+          int c2 = node->getFirstRightChild(b1);
 
           collisionRecurse(node, c1, b2, front_list);
           collisionRecurse(node, c2, b2, front_list);

test/test_fcl_geometric_shapes.cpp

@@ -177,7 +177,7 @@ void printComparisonError(const std::string& comparison_type,
             << "tf2.translation: " << tf2.getTranslation() << std::endl
             << "expected_depth: " << expected_depth << std::endl
             << "actual_depth  : " << actual_depth << std::endl
-            << "difference: " << std::fabs(actual_depth - expected_depth) << std::endl
+            << "difference: " << std::abs(actual_depth - expected_depth) << std::endl
             << "tolerance: " << tol << std::endl;
 }
 
@@ -201,7 +201,7 @@ bool checkContactPoints(const S1& s1, const Transform3f& tf1,
 
   if (check_depth)
   {
-    bool depth_equal = std::fabs(actual.penetration_depth - expected.penetration_depth) < tol;
+    bool depth_equal = std::abs(actual.penetration_depth - expected.penetration_depth) < tol;
     if (!depth_equal)
       return false;
   }