windows CI eigen3.2 sidestepping fix;

check normalize for non-zero lengths and setIdentity for splinemotion transforms
flexible-collision-library · Oct 28, 2020 · 10e12cc · 10e12cc
1 parent a2241df
commit 10e12cc
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/include/fcl/math/motion/spline_motion-inl.h b/include/fcl/math/motion/spline_motion-inl.h
@@ -85,6 +85,7 @@ SplineMotion<S>::SplineMotion(
   RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3;
   RC = (Rd[2] - Rd[0]) * 3;
 
+  tf.setIdentity();
   integrate(0.0);
 }
 
@@ -124,7 +125,8 @@ bool SplineMotion<S>::integrate(S dt) const
   Vector3<S> cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
   Vector3<S> cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
   S cur_angle = cur_w.norm();
-  cur_w.normalize();
+  if (cur_w.squaredNorm() > 0.0)
+    cur_w.normalize();
 
   tf.linear() = AngleAxis<S>(cur_angle, cur_w).toRotationMatrix();
   tf.translation() = cur_T;

diff --git a/...narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h b/...narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h
@@ -77,7 +77,8 @@ leafTesting(int, int) const
   this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2);
 
   Vector3<S> n = closest_p2 - closest_p1;
-  n.normalize();
+  if (n.squaredNorm() > 0.0)
+    n.normalize();
   TBVMotionBoundVisitor<RSS<S>> mb_visitor1(model1_bv, n);
   TBVMotionBoundVisitor<RSS<S>> mb_visitor2(model2_bv, -n);
   S bound1 = motion1->computeMotionBound(mb_visitor1);