Fix shape conservative advancement normal computation (#505)

Fix frame of reference used in computation of n and add test_SplineMotion_rotated_start_test.

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,9 @@ 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_angle > 0.0) {
+ cur_w /= cur_angle;
+ }
 
  tf.linear() = AngleAxis<S>(cur_angle, cur_w).toRotationMatrix();
  tf.translation() = cur_T;

include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h

@@ -76,8 +76,10 @@ leafTesting(int, int) const
  Vector3<S> closest_p2 = Vector3<S>::Zero();
  this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2);
 
- Vector3<S> n = this->tf2 * closest_p2 - this->tf1 * closest_p1;
- n.normalize();
+ Vector3<S> n = closest_p2 - closest_p1;
+ const S norm_sq = n.squaredNorm();
+ if (norm_sq > 0.0)
+ n /= sqrt(norm_sq); // Normalize.
  TBVMotionBoundVisitor<RSS<S>> mb_visitor1(model1_bv, n);
  TBVMotionBoundVisitor<RSS<S>> mb_visitor2(model2_bv, -n);
  S bound1 = motion1->computeMotionBound(mb_visitor1);

test/test_fcl_collision.cpp

@@ -42,6 +42,7 @@
 #include "fcl/narrowphase/detail/gjk_solver_indep.h"
 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
 #include "fcl/narrowphase/detail/traversal/collision_node.h"
+#include "fcl/narrowphase/continuous_collision.h"
 
 #include "test_fcl_utility.h"
 
@@ -87,6 +88,65 @@ std::vector<Contact<S>>& global_pairs_now()
  return static_global_pairs_now;
 }
 
+template <typename S>
+void test_SplineMotion_rotated_spline_collide_test()
+{
+ fcl::Vector3<S> t[4];
+ t[0] = fcl::Vector3<S>(7.5, 8, 0);
+ t[1] = fcl::Vector3<S>(4.2, 8, 0);
+ t[2] = fcl::Vector3<S>(0.8, 8, 0);
+ t[3] = fcl::Vector3<S>(-2.5, 8, 0);
+
+ fcl::Vector3<S> r[4];
+ r[0] = fcl::Vector3<S>(0, 0, 3.141593);
+ r[1] = fcl::Vector3<S>(0, 0, 3.141593);
+ r[2] = fcl::Vector3<S>(0, 0, 3.141593);
+ r[3] = fcl::Vector3<S>(0, 0, 3.141593);
+
+ auto motion_a = fcl::make_aligned_shared<fcl::SplineMotion<S>>(
+ t[0], t[1], t[2], t[3],
+ r[0], r[1], r[2], r[3]);
+
+ t[0] = fcl::Vector3<S>(0.0, 8, 0);
+ t[1] = fcl::Vector3<S>(1.25, 8, 0);
+ t[2] = fcl::Vector3<S>(3.0, 8, 0);
+ t[3] = fcl::Vector3<S>(4.6, 8, 0);
+
+ r[0] = fcl::Vector3<S>(0, 0, 0);
+ r[1] = fcl::Vector3<S>(0, 0, 0);
+ r[2] = fcl::Vector3<S>(0, 0, 0);
+ r[3] = fcl::Vector3<S>(0, 0, 0);
+
+ auto motion_b = fcl::make_aligned_shared<fcl::SplineMotion<S>>(
+ t[0], t[1], t[2], t[3],
+ r[0], r[1], r[2], r[3]);
+
+ // Test collision with unit spheres
+ auto shape_a = std::make_shared<fcl::Sphere<S>>(1.0);
+ const auto obj_a = fcl::ContinuousCollisionObject<S>(
+ shape_a,
+ motion_a);
+
+ auto shape_b = std::make_shared<fcl::Sphere<S>>(1.0);
+ const auto obj_b = fcl::ContinuousCollisionObject<S>(
+ shape_b,
+ motion_b);
+
+ fcl::ContinuousCollisionRequest<S> request;
+ request.ccd_solver_type = fcl::CCDC_CONSERVATIVE_ADVANCEMENT;
+ request.gjk_solver_type = fcl::GST_LIBCCD;
+
+ fcl::ContinuousCollisionResult<S> result;
+ fcl::collide(&obj_a, &obj_b, request, result);
+
+ EXPECT_TRUE(result.is_collide);
+}
+
+GTEST_TEST(FCL_COLLISION, test_SplineMotion_rotated_spline_collide_test)
+{
+ test_SplineMotion_rotated_spline_collide_test<double>();
+}
+
 template <typename S>
 void test_OBB_Box_test()
 {