Support ellipsoid shape

README.md

@@ -12,10 +12,15 @@ FCL has the following features
  - Compilable for either linux or win32 (both makefiles and Microsoft Visual projects can be generated using cmake)
  - No special topological constraints or adjacency information required for input models – all that is necessary is a list of the model's triangles
  - Supported different object shapes:
-  + sphere
   + box
+  + sphere
+  + ellipsoid
+  + capsule
   + cone
   + cylinder
+  + convex
+  + half-space
+  + plane
   + mesh
   + octree (optional, octrees are represented using the octomap library http://octomap.github.com)
 

include/fcl/collision_object.h

@@ -51,9 +51,9 @@ namespace fcl
 /// @brief object type: BVH (mesh, points), basic geometry, octree
 enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
 
-/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
+/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, halfspace, triangle), and octree
 enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
-                GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
+                GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
 
 /// @brief The geometry for the object for collision or distance computation
 class CollisionGeometry

include/fcl/narrowphase/gjk_libccd.h

@@ -97,6 +97,17 @@ class GJKInitializer<Sphere>
   static void deleteGJKObject(void* o);
 };
 
+/// @brief initialize GJK Ellipsoid
+template<>
+class GJKInitializer<Ellipsoid>
+{
+public:
+  static GJKSupportFunction getSupportFunction();
+  static GJKCenterFunction getCenterFunction();
+  static void* createGJKObject(const Ellipsoid& s, const Transform3f& tf);
+  static void deleteGJKObject(void* o);
+};
+
 /// @brief initialize GJK Box
 template<>
 class GJKInitializer<Box>

include/fcl/narrowphase/narrowphase.h

@@ -310,6 +310,16 @@ bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, co
                                                          const Sphere& s2, const Transform3f& tf2,
                                                          std::vector<ContactPoint>* contacts) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Ellipsoid, Halfspace>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                            const Halfspace& s2, const Transform3f& tf2,
+                                                            std::vector<ContactPoint>* contacts) const;
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Ellipsoid>(const Halfspace& s1, const Transform3f& tf1,
+                                                            const Ellipsoid& s2, const Transform3f& tf2,
+                                                            std::vector<ContactPoint>* contacts) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                       const Halfspace& s2, const Transform3f& tf2,
@@ -375,6 +385,16 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Tran
                                                      const Sphere& s2, const Transform3f& tf2,
                                                      std::vector<ContactPoint>* contacts) const;
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Ellipsoid, Plane>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                        const Plane& s2, const Transform3f& tf2,
+                                                        std::vector<ContactPoint>* contacts) const;
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Ellipsoid>(const Plane& s1, const Transform3f& tf1,
+                                                        const Ellipsoid& s2, const Transform3f& tf2,
+                                                        std::vector<ContactPoint>* contacts) const;
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                   const Plane& s2, const Transform3f& tf2,
@@ -883,6 +903,16 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, con
                                                         const Sphere& s2, const Transform3f& tf2,
                                                         std::vector<ContactPoint>* contacts) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Ellipsoid, Halfspace>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                           const Halfspace& s2, const Transform3f& tf2,
+                                                           std::vector<ContactPoint>* contacts) const;
+
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Ellipsoid>(const Halfspace& s1, const Transform3f& tf1,
+                                                           const Ellipsoid& s2, const Transform3f& tf2,
+                                                           std::vector<ContactPoint>* contacts) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                      const Halfspace& s2, const Transform3f& tf2,
@@ -948,6 +978,16 @@ bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Trans
                                                     const Sphere& s2, const Transform3f& tf2,
                                                     std::vector<ContactPoint>* contacts) const;
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Ellipsoid, Plane>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                       const Plane& s2, const Transform3f& tf2,
+                                                       std::vector<ContactPoint>* contacts) const;
+
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Ellipsoid>(const Plane& s1, const Transform3f& tf1,
+                                                       const Ellipsoid& s2, const Transform3f& tf2,
+                                                       std::vector<ContactPoint>* contacts) const;
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                  const Plane& s2, const Transform3f& tf2,

include/fcl/shape/geometric_shape_to_BVH_model.h

@@ -167,6 +167,95 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
   generateBVHModel(model, shape, pose, seg, ring);  
 }
 
+/// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude.
+template<typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid& shape, const Transform3f& pose, unsigned int seg, unsigned int ring)
+{
+  std::vector<Vec3f> points;
+  std::vector<Triangle> tri_indices;
+
+  const FCL_REAL& a = shape.radii[0];
+  const FCL_REAL& b = shape.radii[1];
+  const FCL_REAL& c = shape.radii[2];
+
+  FCL_REAL phi, phid;
+  const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+  phid = pi * 2 / seg;
+  phi = 0;
+
+  FCL_REAL theta, thetad;
+  thetad = pi / (ring + 1);
+  theta = 0;
+
+  for(unsigned int i = 0; i < ring; ++i)
+  {
+    double theta_ = theta + thetad * (i + 1);
+    for(unsigned int j = 0; j < seg; ++j)
+    {
+      points.push_back(Vec3f(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_)));
+    }
+  }
+  points.push_back(Vec3f(0, 0, c));
+  points.push_back(Vec3f(0, 0, -c));
+
+  for(unsigned int i = 0; i < ring - 1; ++i)
+  {
+    for(unsigned int j = 0; j < seg; ++j)
+    {
+       unsigned int a, b, c, d;
+       a = i * seg + j;
+       b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
+       c = (i + 1) * seg + j;
+       d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
+       tri_indices.push_back(Triangle(a, c, b));
+       tri_indices.push_back(Triangle(b, c, d));
+    }
+  }
+
+  for(unsigned int j = 0; j < seg; ++j)
+  {
+    unsigned int a, b;
+    a = j;
+    b = (j == seg - 1) ? 0 : (j + 1);
+    tri_indices.push_back(Triangle(ring * seg, a, b));
+
+    a = (ring - 1) * seg + j;
+    b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
+    tri_indices.push_back(Triangle(a, ring * seg + 1, b));
+  }
+
+  for(unsigned int i = 0; i < points.size(); ++i)
+  {
+    points[i] = pose.transform(points[i]);
+  }
+
+  model.beginModel();
+  model.addSubModel(points, tri_indices);
+  model.endModel();
+  model.computeLocalAABB();
+}
+
+/// @brief Generate BVH model from ellipsoid
+/// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c),
+/// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same.
+/// Reference: https://en.wikipedia.org/wiki/Ellipsoid#Approximate_formula
+template<typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Ellipsoid& shape, const Transform3f& pose, unsigned int n_faces_for_unit_ellipsoid)
+{
+  const FCL_REAL p = 1.6075;
+
+  const FCL_REAL& ap = std::pow(shape.radii[0], p);
+  const FCL_REAL& bp = std::pow(shape.radii[1], p);
+  const FCL_REAL& cp = std::pow(shape.radii[2], p);
+
+  const FCL_REAL ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p);
+  const FCL_REAL n_low_bound = std::sqrt(n_faces_for_unit_ellipsoid / 2.0) * ratio;
+
+  const unsigned int ring = std::ceil(n_low_bound);
+  const unsigned int seg = std::ceil(n_low_bound);
+
+  generateBVHModel(model, shape, pose, seg, ring);
+}
 
 /// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
 template<typename BV>

include/fcl/shape/geometric_shapes.h

@@ -119,14 +119,14 @@ class Sphere : public ShapeBase
   Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
   {
   }
-  
-  /// @brief Radius of the sphere 
+
+  /// @brief Radius of the sphere
   FCL_REAL radius;
 
-  /// @brief Compute AABB 
+  /// @brief Compute AABB
   void computeLocalAABB();
 
-  /// @brief Get node type: a sphere 
+  /// @brief Get node type: a sphere
   NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
 
   Matrix3f computeMomentofInertia() const
@@ -139,7 +139,46 @@ class Sphere : public ShapeBase
 
   FCL_REAL computeVolume() const
   {
-    return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius / 3;
+    return 4.0 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius / 3.0;
+  }
+};
+
+/// @brief Center at zero point ellipsoid
+class Ellipsoid : public ShapeBase
+{
+public:
+  Ellipsoid(FCL_REAL a, FCL_REAL b, FCL_REAL c) : ShapeBase(), radii(a, b, c)
+  {
+  }
+
+  Ellipsoid(const Vec3f& radii_) : ShapeBase(), radii(radii_)
+  {
+  }
+
+  /// @brief Radii of the ellipsoid
+  Vec3f radii;
+
+  /// @brief Compute AABB
+  void computeLocalAABB();
+
+  /// @brief Get node type: a sphere
+  NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; }
+
+  Matrix3f computeMomentofInertia() const
+  {
+    const FCL_REAL V = computeVolume();
+    const FCL_REAL a2 = radii[0] * radii[0] * V;
+    const FCL_REAL b2 = radii[1] * radii[1] * V;
+    const FCL_REAL c2 = radii[2] * radii[2] * V;
+    return Matrix3f(0.2 * (b2 + c2), 0, 0,
+                    0, 0.2 * (a2 + c2), 0,
+                    0, 0, 0.2 * (a2 + b2));
+  }
+
+  FCL_REAL computeVolume() const
+  {
+    const FCL_REAL pi = boost::math::constants::pi<FCL_REAL>();
+    return 4.0 * pi * radii[0] * radii[1] * radii[2] / 3.0;
   }
 };
 

include/fcl/shape/geometric_shapes_utility.h

@@ -52,6 +52,7 @@ namespace details
 /// @brief get the vertices of some convex shape which can bound the given shape in a specific configuration
 std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf);
 std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf);
+std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf);
 std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf);
 std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf);
 std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf);
@@ -75,6 +76,9 @@ void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv);
 template<>
 void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv);
 
+template<>
+void computeBV<AABB, Ellipsoid>(const Ellipsoid& s, const Transform3f& tf, AABB& bv);
+
 template<>
 void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv);
 
@@ -104,6 +108,9 @@ void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv);
 template<>
 void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv);
 
+template<>
+void computeBV<OBB, Ellipsoid>(const Ellipsoid& s, const Transform3f& tf, OBB& bv);
+
 template<>
 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv);