Delete lz for Cone, Cylinder and Capsule, now use halfLength (#98)

include/hpp/fcl/shape/geometric_shapes.h

@@ -151,20 +151,16 @@ public:
 class Capsule : public ShapeBase
 {
 public:
-  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
   {
-    lz = lz_;
-    HalfLength = lz_/2;
+    halfLength = lz_/2;
   }
 
   /// @brief Radius of capsule 
   FCL_REAL radius;
 
-  /// @brief Length along z axis 
-  FCL_REAL lz;
-
   /// @brief Half Length along z axis 
-  FCL_REAL HalfLength;
+  FCL_REAL halfLength;
 
   /// @brief Compute AABB 
   void computeLocalAABB();
@@ -174,15 +170,15 @@ public:
 
   FCL_REAL computeVolume() const
   {
-    return boost::math::constants::pi<FCL_REAL>() * radius * radius *(lz + radius * 4/3.0);
+    return boost::math::constants::pi<FCL_REAL>() * radius * radius *((halfLength * 2) + radius * 4/3.0);
   }
 
   Matrix3f computeMomentofInertia() const
   {
-    FCL_REAL v_cyl = radius * radius * lz * boost::math::constants::pi<FCL_REAL>();
+    FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi<FCL_REAL>();
     FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
     
-    FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz;
+    FCL_REAL ix = v_cyl * halfLength * halfLength / 3. + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + v_sph * halfLength * halfLength;
     FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
 
     return (Matrix3f() << ix, 0, 0,
@@ -196,20 +192,16 @@ public:
 class Cone : public ShapeBase
 {
 public:
-  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
   {
-    lz = lz_;
-    HalfLength = lz_/2;
+    halfLength = lz_/2;
   }
 
   /// @brief Radius of the cone 
   FCL_REAL radius;
 
-  /// @brief Length along z axis 
-  FCL_REAL lz;
-
   /// @brief Half Length along z axis 
-  FCL_REAL HalfLength;
+  FCL_REAL halfLength;
 
   /// @brief Compute AABB 
   void computeLocalAABB();
@@ -219,13 +211,14 @@ public:
 
   FCL_REAL computeVolume() const
   {
-    return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz / 3;
+    return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2) / 3;
   }
 
+  /// \todo verify this formula as it seems different from https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors
   Matrix3f computeMomentofInertia() const
   {
     FCL_REAL V = computeVolume();
-    FCL_REAL ix = V * (0.1 * lz * lz + 3 * radius * radius / 20);
+    FCL_REAL ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
     FCL_REAL iz = 0.3 * V * radius * radius;
 
     return (Matrix3f() << ix, 0, 0,
@@ -235,7 +228,7 @@ public:
 
   Vec3f computeCOM() const
   {
-    return Vec3f(0, 0, -0.25 * lz);
+    return Vec3f(0, 0, -0.5 * halfLength);
   }
 };
 
@@ -243,20 +236,16 @@ public:
 class Cylinder : public ShapeBase
 {
 public:
-  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
   {
-    lz = lz_;
-    HalfLength = lz_/2;
+    halfLength = lz_/2;
   }
   
   /// @brief Radius of the cylinder 
   FCL_REAL radius;
 
-  /// @brief Length along z axis 
-  FCL_REAL lz;
-
   /// @brief Half Length along z axis 
-  FCL_REAL HalfLength;
+  FCL_REAL halfLength;
 
   /// @brief Compute AABB 
   void computeLocalAABB();
@@ -266,13 +255,13 @@ public:
 
   FCL_REAL computeVolume() const
   {
-    return boost::math::constants::pi<FCL_REAL>() * radius * radius * lz;
+    return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2);
   }
 
   Matrix3f computeMomentofInertia() const
   {
     FCL_REAL V = computeVolume();
-    FCL_REAL ix = V * (3 * radius * radius + lz * lz) / 12;
+    FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
     FCL_REAL iz = V * radius * radius / 2;
     return (Matrix3f() << ix, 0, 0,
                           0, ix, 0,

python/collision-geometries.cc

@@ -122,13 +122,13 @@ void exposeShapes ()
   class_ <Capsule, bases<ShapeBase>, shared_ptr<Capsule> >
     ("Capsule", init<FCL_REAL, FCL_REAL>())
     .def_readwrite ("radius", &Capsule::radius)
-    .def_readwrite ("lz", &Capsule::lz)
+    .def_readwrite ("halfLength", &Capsule::halfLength)
     ;
 
   class_ <Cone, bases<ShapeBase>, shared_ptr<Cone> >
     ("Cone", init<FCL_REAL, FCL_REAL>())
     .def_readwrite ("radius", &Cone::radius)
-    .def_readwrite ("lz", &Cone::lz)
+    .def_readwrite ("halfLength", &Cone::halfLength)
     ;
 
   class_ <ConvexBase, bases<ShapeBase>, shared_ptr<ConvexBase >, noncopyable>
@@ -148,7 +148,7 @@ void exposeShapes ()
   class_ <Cylinder, bases<ShapeBase>, shared_ptr<Cylinder> >
     ("Cylinder", init<FCL_REAL, FCL_REAL>())
     .def_readwrite ("radius", &Cylinder::radius)
-    .def_readwrite ("lz", &Cylinder::lz)
+    .def_readwrite ("halfLength", &Cylinder::halfLength)
     ;
 
   class_ <Halfspace, bases<ShapeBase>, shared_ptr<Halfspace> >

src/distance_capsule_capsule.cpp

@@ -41,8 +41,8 @@ namespace fcl {
     // We assume that capsules are oriented along z-axis.
     Matrix3f::ConstColXpr direction1 = tf1.getRotation ().col (2);
     Matrix3f::ConstColXpr direction2 = tf2.getRotation ().col (2);
-    FCL_REAL halfLength1 = 0.5*c1->lz;
-    FCL_REAL halfLength2 = 0.5*c2->lz;
+    FCL_REAL halfLength1 = c1->halfLength;
+    FCL_REAL halfLength2 = c2->halfLength;
 
     Vec3f diff = center1 - center2;
     FCL_REAL a01 = -direction1.dot (direction2);

src/narrowphase/details.h

@@ -73,8 +73,8 @@ namespace fcl {
        const Capsule& s2, const Transform3f& tf2,
        Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
     {
-      Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz))); // from distance function
-      Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
+      Vec3f pos1 (tf2.transform (Vec3f (0., 0.,  s2.halfLength))); // from distance function
+      Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
       Vec3f s_c = tf1.getTranslation ();
 
       Vec3f segment_point;
@@ -108,8 +108,8 @@ namespace fcl {
        const Capsule& s2, const Transform3f& tf2,
        FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
     {
-      Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz)));
-      Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
+      Vec3f pos1 (tf2.transform (Vec3f (0., 0.,  s2.halfLength)));
+      Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
       Vec3f s_c = tf1.getTranslation ();
 
       Vec3f segment_point;
@@ -143,7 +143,7 @@ namespace fcl {
       static const FCL_REAL eps (sqrt (std::numeric_limits <FCL_REAL>::epsilon ()));
       FCL_REAL r1 (s1.radius);
       FCL_REAL r2 (s2.radius);
-      FCL_REAL lz2 (.5*s2.lz);
+      FCL_REAL lz2 (s2.halfLength);
       // boundaries of the cylinder axis
       Vec3f A (tf2.transform (Vec3f (0, 0, -lz2)));
       Vec3f B (tf2.transform (Vec3f (0, 0,  lz2)));
@@ -151,7 +151,7 @@ namespace fcl {
       Vec3f S (tf1.getTranslation ());
       // axis of the cylinder
       Vec3f u (tf2.getRotation ().col (2));
-      assert ((B - A - s2.lz * u).norm () < eps);
+      assert ((B - A - (s2.halfLength * 2) * u).norm () < eps);
       Vec3f AS (S - A);
       // abscissa of S on cylinder axis with A as the origin
       FCL_REAL s (u.dot (AS));
@@ -186,7 +186,7 @@ namespace fcl {
             dist = -r1;
           }
         }
-      } else if (s <= s2.lz) {
+      } else if (s <= (s2.halfLength * 2)) {
         // 0 < s <= s2.lz
         normal = -v;
         dist = dPS - r1 - r2;
@@ -200,7 +200,7 @@ namespace fcl {
         // lz < s
         if (dPS <= r2) {
           // closest point on cylinder is on cylinder disc basis
-          dist = s - s2.lz - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u;
+          dist = s - (s2.halfLength * 2) - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u;
         } else {
           // closest point on cylinder is on cylinder circle basis
           p2 = B + r2 * v;
@@ -1605,7 +1605,7 @@ namespace fcl {
           int sign = (cosa > 0) ? -1 : 1;
           // closest capsule vertex to halfspace if no collision,
           // or deeper inside halspace if collision
-          Vec3f p = T + dir_z * (s1.lz * 0.5 * sign);
+          Vec3f p = T + dir_z * (s1.halfLength * sign);
 
           FCL_REAL signed_dist = new_s2.signedDistance(p);
           distance = signed_dist - s1.radius;
@@ -1666,7 +1666,7 @@ namespace fcl {
 
           int sign = (cosa > 0) ? -1 : 1;
           // deepest point
-          Vec3f p = T + dir_z * (s1.lz * 0.5 * sign) + C;
+          Vec3f p = T + dir_z * (s1.halfLength * sign) + C;
           distance = new_s2.signedDistance(p);
           if(distance > 0) {
             // TODO: compute closest points
@@ -1708,7 +1708,7 @@ namespace fcl {
           else
             {
               normal = -new_s2.n;
-              p1 = p2 = T - dir_z * (s1.lz * 0.5) -
+              p1 = p2 = T - dir_z * (s1.halfLength) -
                 new_s2.n * (0.5 * distance + s1.radius);
               return true;
             }
@@ -1726,8 +1726,8 @@ namespace fcl {
               C *= s;
             }
 
-          Vec3f a1 = T + dir_z * (0.5 * s1.lz);
-          Vec3f a2 = T - dir_z * (0.5 * s1.lz) + C;
+          Vec3f a1 = T + dir_z * (s1.halfLength);
+          Vec3f a2 = T - dir_z * (s1.halfLength) + C;
 
           FCL_REAL d1 = new_s2.signedDistance(a1);
           FCL_REAL d2 = new_s2.signedDistance(a2);
@@ -2153,8 +2153,8 @@ namespace fcl {
       Vec3f dir_z = R1.col(2);
 
       // ends of capsule inner segment
-      Vec3f a1 = T1 + dir_z * (0.5 * s1.lz);
-      Vec3f a2 = T1 - dir_z * (0.5 * s1.lz);
+      Vec3f a1 = T1 + dir_z * s1.halfLength;
+      Vec3f a2 = T1 - dir_z * s1.halfLength;
 
       FCL_REAL d1 = new_s2.signedDistance(a1);
       FCL_REAL d2 = new_s2.signedDistance(a2);
@@ -2278,8 +2278,8 @@ namespace fcl {
           C *= s;
         }
 
-        Vec3f a1 = T + dir_z * (0.5 * s1.lz);
-        Vec3f a2 = T - dir_z * (0.5 * s1.lz);
+        Vec3f a1 = T + dir_z * (s1.halfLength);
+        Vec3f a2 = T - dir_z * (s1.halfLength);
 
         Vec3f c1, c2;
         if(cosa > 0)
@@ -2346,8 +2346,8 @@ namespace fcl {
           else
             {
               if (d < 0) normal = new_s2.n; else normal = -new_s2.n;
-              p1 = p2 = T - dir_z * (0.5 * s1.lz) +
-                dir_z * (-0.5 * distance / s1.radius * s1.lz) - new_s2.n * d;
+              p1 = p2 = T - dir_z * (s1.halfLength) +
+                dir_z * (- distance / s1.radius * s1.halfLength) - new_s2.n * d;
               return true;
             }
         }
@@ -2365,9 +2365,9 @@ namespace fcl {
             }
 
           Vec3f c[3];
-          c[0] = T + dir_z * (0.5 * s1.lz);
-          c[1] = T - dir_z * (0.5 * s1.lz) + C;
-          c[2] = T - dir_z * (0.5 * s1.lz) - C;
+          c[0] = T + dir_z * (s1.halfLength);
+          c[1] = T - dir_z * (s1.halfLength) + C;
+          c[2] = T - dir_z * (s1.halfLength) - C;
 
           FCL_REAL d[3];
           d[0] = new_s2.signedDistance(c[0]);

src/narrowphase/gjk.cpp

@@ -119,8 +119,8 @@ inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& suppo
 inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support)
 {
   support = capsule->radius * dir;
-  if (dir[2] > 0) support[2] += capsule->lz / 2;
-  else            support[2] -= capsule->lz / 2;
+  if (dir[2] > 0) support[2] += capsule->halfLength;
+  else            support[2] -= capsule->halfLength;
 }
 
 void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
@@ -137,7 +137,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
   FCL_REAL len = zdist + dir[2] * dir[2];
   zdist = std::sqrt(zdist);
   len = std::sqrt(len);
-  FCL_REAL half_h = cone->lz * 0.5;
+  FCL_REAL half_h = cone->halfLength;
   FCL_REAL radius = cone->radius;
 
   FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
@@ -156,7 +156,7 @@ 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 half_h = cylinder->lz * 0.5;
+  FCL_REAL half_h = cylinder->halfLength;
   if      (dir [2] >  eps) support[2] =  half_h;
   else if (dir [2] < -eps) support[2] = -half_h;
   else                     support[2] = 0;

src/shape/geometric_shapes_utility.cpp

@@ -96,7 +96,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& t
   std::vector<Vec3f> result(36);
   const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
 
-  FCL_REAL hl = capsule.lz * 0.5;
+  FCL_REAL hl = capsule.halfLength;
   FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0));
   FCL_REAL a = edge_size;
   FCL_REAL b = m * edge_size;
@@ -153,7 +153,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
 {
   std::vector<Vec3f> result(7);
   
-  FCL_REAL hl = cone.lz * 0.5;
+  FCL_REAL hl = cone.halfLength;
   FCL_REAL r2 = cone.radius * 2 / sqrt(3.0);
   FCL_REAL a = 0.5 * r2;
   FCL_REAL b = cone.radius;
@@ -174,7 +174,7 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f&
 {
   std::vector<Vec3f> result(12);
 
-  FCL_REAL hl = cylinder.lz * 0.5;
+  FCL_REAL hl = cylinder.halfLength;
   FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
   FCL_REAL a = 0.5 * r2;
   FCL_REAL b = cylinder.radius;
@@ -277,7 +277,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
   const Matrix3f& R = tf.getRotation();
   const Vec3f& T = tf.getTranslation();
 
-  Vec3f v_delta(0.5 * (R.col(2)*s.lz).cwiseAbs() + Vec3f::Constant(s.radius));
+  Vec3f v_delta(R.col(2).cwiseAbs() * s.halfLength + Vec3f::Constant(s.radius));
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
@@ -288,9 +288,9 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
   const Matrix3f& R = tf.getRotation();
   const Vec3f& T = tf.getTranslation();
 
-  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
-  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
-  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
+  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength);
+  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength);
+  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength);
 
   Vec3f v_delta(x_range, y_range, z_range);
   bv.max_ = T + v_delta;
@@ -303,9 +303,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& b
   const Matrix3f& R = tf.getRotation();
   const Vec3f& T = tf.getTranslation();
 
-  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz);
-  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz);
-  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz);
+  FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + fabs(R(0, 2) * s.halfLength);
+  FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + fabs(R(1, 2) * s.halfLength);
+  FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + fabs(R(2, 2) * s.halfLength);