[GJK] Avoid normalization for Cylinder, Convex and Cone.

src/narrowphase/gjk.cpp

@@ -67,6 +67,24 @@ template <> struct shape_traits<Box> : shape_traits_base
   };
 };
 
+template <> struct shape_traits<Cone> : shape_traits_base
+{
+  enum { NeedNormalizedDir = false
+  };
+};
+
+template <> struct shape_traits<Cylinder> : shape_traits_base
+{
+  enum { NeedNormalizedDir = false
+  };
+};
+
+template <> struct shape_traits<Convex> : shape_traits_base
+{
+  enum { NeedNormalizedDir = false
+  };
+};
+
 void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support)
 {
   FCL_REAL dota = dir.dot(triangle->a);
@@ -138,17 +156,14 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
 void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
 {
   static const FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
-  FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
   FCL_REAL half_h = cylinder->lz * 0.5;
-  if(zdist == 0.0)
-    support = Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
-  else {
-    FCL_REAL d = cylinder->radius / zdist;
-    FCL_REAL z (0.);
-    if (dir [2] > eps) z = half_h;
-    else if (dir [2] < -eps) z = -half_h;
-    support << d * dir.head<2>(), z;
-  }
+  if      (dir [2] >  eps) support[2] =  half_h;
+  else if (dir [2] < -eps) support[2] = -half_h;
+  else                     support[2] = 0;
+  if (dir.head<2>().isZero())
+    support.head<2>().setZero();
+  else
+    support.head<2>() = dir.head<2>().normalized() * cylinder->radius;
   assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps);
 }