From 10e12cc66c2de6002b9bff344c0f8e7dd0a92926 Mon Sep 17 00:00:00 2001 From: ddeng Date: Wed, 28 Oct 2020 11:37:09 +0800 Subject: [PATCH] windows CI eigen3.2 sidestepping fix; check normalize for non-zero lengths and setIdentity for splinemotion transforms --- include/fcl/math/motion/spline_motion-inl.h | 4 +++- .../shape_conservative_advancement_traversal_node-inl.h | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/include/fcl/math/motion/spline_motion-inl.h b/include/fcl/math/motion/spline_motion-inl.h index 9986aecb0..13b8fec6a 100644 --- a/include/fcl/math/motion/spline_motion-inl.h +++ b/include/fcl/math/motion/spline_motion-inl.h @@ -85,6 +85,7 @@ SplineMotion::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::integrate(S dt) const Vector3 cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt); Vector3 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(cur_angle, cur_w).toRotationMatrix(); tf.translation() = cur_T; diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h index 8aad5d39b..5070f3b9c 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h +++ b/include/fcl/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 n = closest_p2 - closest_p1; - n.normalize(); + if (n.squaredNorm() > 0.0) + n.normalize(); TBVMotionBoundVisitor> mb_visitor1(model1_bv, n); TBVMotionBoundVisitor> mb_visitor2(model2_bv, -n); S bound1 = motion1->computeMotionBound(mb_visitor1);