diff --git a/README.md b/README.md
index 6cfa0a149..24c3b07e3 100644
--- a/README.md
+++ b/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)
 
diff --git a/include/fcl/collision_object.h b/include/fcl/collision_object.h
index 7cc2fc5fe..0e41fbb16 100644
--- a/include/fcl/collision_object.h
+++ b/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
diff --git a/include/fcl/narrowphase/gjk_libccd.h b/include/fcl/narrowphase/gjk_libccd.h
index c5c7f09a8..ca8c3b2a8 100644
--- a/include/fcl/narrowphase/gjk_libccd.h
+++ b/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>
diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
old mode 100644
new mode 100755
index ef4860081..de15b6df2
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/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,
diff --git a/include/fcl/shape/geometric_shape_to_BVH_model.h b/include/fcl/shape/geometric_shape_to_BVH_model.h
index a07022b85..d729707bd 100644
--- a/include/fcl/shape/geometric_shape_to_BVH_model.h
+++ b/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>
diff --git a/include/fcl/shape/geometric_shapes.h b/include/fcl/shape/geometric_shapes.h
index 1b1070e5d..6ac9146cb 100644
--- a/include/fcl/shape/geometric_shapes.h
+++ b/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;
   }
 };
 
diff --git a/include/fcl/shape/geometric_shapes_utility.h b/include/fcl/shape/geometric_shapes_utility.h
index 6feab73e7..05c49142c 100644
--- a/include/fcl/shape/geometric_shapes_utility.h
+++ b/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);
 
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
old mode 100644
new mode 100755
index 32ce18e7b..1d9af8873
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -455,6 +455,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide<Box, Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder, NarrowPhaseSolver>;
@@ -464,6 +465,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Sphere, Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder, NarrowPhaseSolver>;
@@ -471,8 +473,19 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
   collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere, Halfspace, NarrowPhaseSolver>;
 
+  collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide<Ellipsoid, Box, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide<Ellipsoid, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide<Ellipsoid, Ellipsoid, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeCollide<Ellipsoid, Capsule, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeCollide<Ellipsoid, Cone, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeCollide<Ellipsoid, Cylinder, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeCollide<Ellipsoid, Convex, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeCollide<Ellipsoid, Plane, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide<Ellipsoid, Halfspace, NarrowPhaseSolver>;
+
   collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Capsule, Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder, NarrowPhaseSolver>;
@@ -482,6 +495,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Cone, Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder, NarrowPhaseSolver>;
@@ -491,6 +505,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide<Cylinder, Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder, NarrowPhaseSolver>;
@@ -500,6 +515,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeCollide<Convex, Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex, Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex, Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder, NarrowPhaseSolver>;
@@ -509,6 +525,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Plane, Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder, NarrowPhaseSolver>;
@@ -527,6 +544,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider<AABB, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder, NarrowPhaseSolver>::collide;
@@ -536,6 +554,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider<OBB, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder, NarrowPhaseSolver>::collide;
@@ -545,6 +564,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider<RSS, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder, NarrowPhaseSolver>::collide;
@@ -554,6 +574,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<16>, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder, NarrowPhaseSolver>::collide;
@@ -563,6 +584,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<18>, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder, NarrowPhaseSolver>::collide;
@@ -572,6 +594,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<24>, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder, NarrowPhaseSolver>::collide;
@@ -581,6 +604,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider<kIOS, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder, NarrowPhaseSolver>::collide;
@@ -590,6 +614,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere, NarrowPhaseSolver>::collide;
+  collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider<OBBRSS, Ellipsoid, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS, Capsule, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS, Cone, NarrowPhaseSolver>::collide;
   collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder, NarrowPhaseSolver>::collide;
@@ -609,6 +634,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 #if FCL_HAVE_OCTOMAP
   collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeCollide<Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder, NarrowPhaseSolver>;
@@ -618,6 +644,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box, NarrowPhaseSolver>;
   collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere, NarrowPhaseSolver>;
+  collision_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeCollide<Ellipsoid, NarrowPhaseSolver>;
   collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule, NarrowPhaseSolver>;
   collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone, NarrowPhaseSolver>;
   collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder, NarrowPhaseSolver>;
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
old mode 100644
new mode 100755
index a83c114a3..7d120a0ef
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -297,6 +297,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance<Box, Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder, NarrowPhaseSolver>;
@@ -306,6 +307,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Sphere, Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere, Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder, NarrowPhaseSolver>;
@@ -313,8 +315,19 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere, Halfspace, NarrowPhaseSolver>;
 
+  distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance<Ellipsoid, Box, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance<Ellipsoid, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance<Ellipsoid, Ellipsoid, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeDistance<Ellipsoid, Capsule, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeDistance<Ellipsoid, Cone, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeDistance<Ellipsoid, Cylinder, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeDistance<Ellipsoid, Convex, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance<Ellipsoid, Plane, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance<Ellipsoid, Halfspace, NarrowPhaseSolver>;
+
   distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Capsule, Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule, Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder, NarrowPhaseSolver>;
@@ -324,6 +337,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Cone, Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder, NarrowPhaseSolver>;
@@ -333,6 +347,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance<Cylinder, Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder, Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder, Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder, NarrowPhaseSolver>;
@@ -342,6 +357,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeDistance<Convex, Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<Convex, Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<Convex, Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex, Cylinder, NarrowPhaseSolver>;
@@ -351,6 +367,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Plane, Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane, Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder, NarrowPhaseSolver>;
@@ -371,6 +388,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   /*
   distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeDistancer<AABB, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder, NarrowPhaseSolver>::distance;
@@ -380,6 +398,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer<OBB, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder, NarrowPhaseSolver>::distance;
@@ -390,6 +409,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer<RSS, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder, NarrowPhaseSolver>::distance;
@@ -401,6 +421,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
   /*
   distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<16>, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<16>, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<16>, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder, NarrowPhaseSolver>::distance;
@@ -410,6 +431,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<18>, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<18>, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder, NarrowPhaseSolver>::distance;
@@ -419,6 +441,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<24>, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<24>, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder, NarrowPhaseSolver>::distance;
@@ -429,6 +452,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer<kIOS, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder, NarrowPhaseSolver>::distance;
@@ -438,6 +462,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere, NarrowPhaseSolver>::distance;
+  distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer<OBBRSS, Ellipsoid, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS, Capsule, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS, Cone, NarrowPhaseSolver>::distance;
   distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder, NarrowPhaseSolver>::distance;
@@ -453,6 +478,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 #if FCL_HAVE_OCTOMAP
   distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeDistance<Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder, NarrowPhaseSolver>;
@@ -462,6 +488,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
 
   distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box, NarrowPhaseSolver>;
   distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere, NarrowPhaseSolver>;
+  distance_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeDistance<Ellipsoid, NarrowPhaseSolver>;
   distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule, NarrowPhaseSolver>;
   distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone, NarrowPhaseSolver>;
   distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder, NarrowPhaseSolver>;
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index c880df851..01f0535c7 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -84,6 +84,20 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
       return dir * sphere->radius;
     }
     break;
+  case GEOM_ELLIPSOID:
+    {
+      const Ellipsoid* ellipsoid = static_cast<const Ellipsoid*>(shape);
+
+      const FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0];
+      const FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1];
+      const FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2];
+
+      const Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]);
+      const FCL_REAL d = std::sqrt(v.dot(dir));
+
+      return v / d;
+    }
+    break;
   case GEOM_CAPSULE:
     {
       const Capsule* capsule = static_cast<const Capsule*>(shape);
diff --git a/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp
index 5f19800c2..f159965cf 100644
--- a/src/narrowphase/gjk_libccd.cpp
+++ b/src/narrowphase/gjk_libccd.cpp
@@ -78,6 +78,11 @@ struct ccd_sphere_t : public ccd_obj_t
   ccd_real_t radius;
 };
 
+struct ccd_ellipsoid_t : public ccd_obj_t
+{
+  ccd_real_t radii[3];
+};
+
 struct ccd_convex_t : public ccd_obj_t
 {
   const Convex* convex;
@@ -548,6 +553,14 @@ static void sphereToGJK(const Sphere& s, const Transform3f& tf, ccd_sphere_t* sp
   sph->radius = s.radius;
 }
 
+static void ellipsoidToGJK(const Ellipsoid& s, const Transform3f& tf, ccd_ellipsoid_t* ellipsoid)
+{
+  shapeToGJK(s, tf, ellipsoid);
+  ellipsoid->radii[0] = s.radii[0];
+  ellipsoid->radii[1] = s.radii[1];
+  ellipsoid->radii[2] = s.radii[2];
+}
+
 static void convexToGJK(const Convex& s, const Transform3f& tf, ccd_convex_t* conv)
 {
   shapeToGJK(s, tf, conv);
@@ -670,6 +683,30 @@ static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v
   ccdVec3Add(v, &s->pos);
 }
 
+static void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+  const ccd_ellipsoid_t* s = static_cast<const ccd_ellipsoid_t*>(obj);
+  ccd_vec3_t dir;
+
+  ccdVec3Copy(&dir, dir_);
+  ccdQuatRotVec(&dir, &s->rot_inv);
+
+  ccd_vec3_t abc2;
+  abc2.v[0] = s->radii[0] * s->radii[0];
+  abc2.v[1] = s->radii[1] * s->radii[1];
+  abc2.v[2] = s->radii[2] * s->radii[2];
+
+  v->v[0] = abc2.v[0] * dir.v[0];
+  v->v[1] = abc2.v[1] * dir.v[1];
+  v->v[2] = abc2.v[2] * dir.v[2];
+
+  ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Dot(v, &dir)));
+
+  // transform support vertex
+  ccdQuatRotVec(v, &s->rot);
+  ccdVec3Add(v, &s->pos);
+}
+
 static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
 {
   const ccd_convex_t* c = (const ccd_convex_t*)obj;
@@ -868,6 +905,29 @@ void GJKInitializer<Sphere>::deleteGJKObject(void* o_)
   delete o;
 }
 
+GJKSupportFunction GJKInitializer<Ellipsoid>::getSupportFunction()
+{
+  return &supportEllipsoid;
+}
+
+GJKCenterFunction GJKInitializer<Ellipsoid>::getCenterFunction()
+{
+  return &centerShape;
+}
+
+void* GJKInitializer<Ellipsoid>::createGJKObject(const Ellipsoid& s, const Transform3f& tf)
+{
+  ccd_ellipsoid_t* o = new ccd_ellipsoid_t;
+  ellipsoidToGJK(s, tf, o);
+  return o;
+}
+
+void GJKInitializer<Ellipsoid>::deleteGJKObject(void* o_)
+{
+  ccd_ellipsoid_t* o = static_cast<ccd_ellipsoid_t*>(o_);
+  delete o;
+}
+
 GJKSupportFunction GJKInitializer<Box>::getSupportFunction()
 {
   return &supportBox;
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
old mode 100644
new mode 100755
index 6bcec4886..11fef9824
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -1531,6 +1531,46 @@ bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1,
   }
 }
 
+bool ellipsoidHalfspaceIntersect(const Ellipsoid& s1, const Transform3f& tf1,
+                                 const Halfspace& s2, const Transform3f& tf2,
+                                 std::vector<ContactPoint>* contacts)
+{
+  // We first compute a single contact in the ellipsoid coordinates, tf1, then
+  // will transform it to the world frame. So we use a new halfspace that is
+  // expressed in the ellipsoid coordinates.
+  const Halfspace& new_s2 = transform(s2, inverse(tf1) * tf2);
+
+  // Compute distance between the ellipsoid's center and a contact plane, whose
+  // normal is equal to the halfspace's normal.
+  const Vec3f normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2));
+  const Vec3f radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2));
+  const FCL_REAL center_to_contact_plane = std::sqrt(normal2.dot(radii2));
+
+  // Depth is the distance between the contact plane and the halfspace.
+  const FCL_REAL depth = center_to_contact_plane + new_s2.d;
+
+  if (depth >= 0)
+  {
+    if (contacts)
+    {
+      // Transform the results to the world coordinates.
+      const Vec3f normal = tf1.getRotation() * -new_s2.n; // pointing from s1 to s2
+      const Vec3f support_vector = (1.0/center_to_contact_plane) * Vec3f(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]);
+      const Vec3f point_in_halfspace_coords = support_vector * (0.5 * depth / new_s2.n.dot(support_vector) - 1.0);
+      const Vec3f point = tf1.transform(point_in_halfspace_coords); // roughly speaking, a middle point of the intersecting volume
+      const FCL_REAL penetration_depth = depth;
+
+      contacts->push_back(ContactPoint(normal, point, penetration_depth));
+    }
+
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
 /// @brief box half space, a, b, c  = +/- edge size
 /// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d
 /// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d
@@ -2045,6 +2085,48 @@ bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1,
   }
 }
 
+bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3f& tf1,
+                             const Plane& s2, const Transform3f& tf2,
+                             std::vector<ContactPoint>* contacts)
+{
+  // We first compute a single contact in the ellipsoid coordinates, tf1, then
+  // will transform it to the world frame. So we use a new plane that is
+  // expressed in the ellipsoid coordinates.
+  const Plane& new_s2 = transform(s2, inverse(tf1) * tf2);
+
+  // Compute distance between the ellipsoid's center and a contact plane, whose
+  // normal is equal to the plane's normal.
+  const Vec3f normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2));
+  const Vec3f radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2));
+  const FCL_REAL center_to_contact_plane = std::sqrt(normal2.dot(radii2));
+
+  const FCL_REAL signed_dist = -new_s2.d;
+
+  // Depth is the distance between the contact plane and the given plane.
+  const FCL_REAL depth = center_to_contact_plane - std::abs(signed_dist);
+
+  if (depth >= 0)
+  {
+    if (contacts)
+    {
+      // Transform the results to the world coordinates.
+      const Vec3f normal = (signed_dist > 0) ? tf1.getRotation() * -new_s2.n : tf1.getRotation() * new_s2.n; // pointing from the ellipsoid's center to the plane
+      const Vec3f support_vector = (1.0/center_to_contact_plane) * Vec3f(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]);
+      const Vec3f point_in_plane_coords = support_vector * (depth / new_s2.n.dot(support_vector) - 1.0);
+      const Vec3f point = (signed_dist > 0) ? tf1.transform(point_in_plane_coords) : tf1.transform(-point_in_plane_coords); // a middle point of the intersecting volume
+      const FCL_REAL penetration_depth = depth;
+
+      contacts->push_back(ContactPoint(normal, point, penetration_depth));
+    }
+
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
 /// @brief box half space, a, b, c  = +/- edge size
 /// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d
 /// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d
@@ -2638,25 +2720,27 @@ bool planeIntersect(const Plane& s1, const Transform3f& tf1,
 
 // Shape intersect algorithms not using libccd
 //
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | box        |  O  |        |         |      |          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | sphere     |/////|   O    |    O    |      |          |   O   |      O     |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | capsule    |/////|////////|         |      |          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cone       |/////|////////|/////////|      |          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cylinder   |/////|////////|/////////|//////|          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | plane      |/////|////////|/////////|//////|//////////|   O   |      O     |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | half-space |/////|////////|/////////|//////|//////////|///////|      O     |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |        |           |         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |    O    |      |          |   O   |      O     |    O     |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|           |         |      |          |   O   |      O     |   TODO   |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|   O   |      O     |    O     |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|      O     |    O     |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
 
 void flipNormal(std::vector<ContactPoint>& contacts)
 {
@@ -2716,6 +2800,24 @@ bool GJKSolver_libccd::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, co
   return res;
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Ellipsoid, Halfspace>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                            const Halfspace& s2, const Transform3f& tf2,
+                                                            std::vector<ContactPoint>* contacts) const
+{
+  return details::ellipsoidHalfspaceIntersect(s1, tf1, s2, tf2, contacts);
+}
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Halfspace, Ellipsoid>(const Halfspace& s1, const Transform3f& tf1,
+                                                            const Ellipsoid& s2, const Transform3f& tf2,
+                                                            std::vector<ContactPoint>* contacts) const
+{
+  const bool res = details::ellipsoidHalfspaceIntersect(s2, tf2, s1, tf1, contacts);
+  if (contacts) flipNormal(*contacts);
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                       const Halfspace& s2, const Transform3f& tf2,
@@ -2842,6 +2944,24 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Sphere>(const Plane& s1, const Tran
   return res;
 }
 
+template<>
+bool GJKSolver_libccd::shapeIntersect<Ellipsoid, Plane>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                        const Plane& s2, const Transform3f& tf2,
+                                                        std::vector<ContactPoint>* contacts) const
+{
+  return details::ellipsoidPlaneIntersect(s1, tf1, s2, tf2, contacts);
+}
+
+template<>
+bool GJKSolver_libccd::shapeIntersect<Plane, Ellipsoid>(const Plane& s1, const Transform3f& tf1,
+                                                        const Ellipsoid& s2, const Transform3f& tf2,
+                                                        std::vector<ContactPoint>* contacts) const
+{
+  const bool res = details::ellipsoidPlaneIntersect(s2, tf2, s1, tf1, contacts);
+  if (contacts) flipNormal(*contacts);
+  return res;
+}
+
 template<>
 bool GJKSolver_libccd::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                   const Plane& s2, const Transform3f& tf2,
@@ -2955,25 +3075,27 @@ bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f&
 
 // Shape distance algorithms not using libccd
 //
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | box        |     |        |         |      |          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | sphere     |/////|   O    |    O    |      |          |       |            |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | capsule    |/////|////////|    O    |      |          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cone       |/////|////////|/////////|      |          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cylinder   |/////|////////|/////////|//////|          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | plane      |/////|////////|/////////|//////|//////////|       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | half-space |/////|////////|/////////|//////|//////////|///////|            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |     |        |           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |    O    |      |          |       |            |     O    |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|    O    |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
 
 template<>
 bool GJKSolver_libccd::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
@@ -3028,25 +3150,27 @@ bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Tran
 
 // Shape intersect algorithms not using built-in GJK algorithm
 //
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | box        |  O  |        |         |      |          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | sphere     |/////|   O    |    O    |      |          |   O   |      O     |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | capsule    |/////|////////|         |      |          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cone       |/////|////////|/////////|      |          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cylinder   |/////|////////|/////////|//////|          |   O   |      O     |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | plane      |/////|////////|/////////|//////|//////////|   O   |      O     |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | half-space |/////|////////|/////////|//////|//////////|///////|      O     |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |        |           |         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |    O    |      |          |   O   |      O     |     O    |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|           |         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|   O   |      O     |     O    |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|      O     |     O    |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
 
 template<>
 bool GJKSolver_indep::shapeIntersect<Sphere, Capsule>(const Sphere &s1, const Transform3f& tf1,
@@ -3100,6 +3224,24 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, con
   return res;
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Ellipsoid, Halfspace>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                           const Halfspace& s2, const Transform3f& tf2,
+                                                           std::vector<ContactPoint>* contacts) const
+{
+  return details::ellipsoidHalfspaceIntersect(s1, tf1, s2, tf2, contacts);
+}
+
+template<>
+bool GJKSolver_indep::shapeIntersect<Halfspace, Ellipsoid>(const Halfspace& s1, const Transform3f& tf1,
+                                                           const Ellipsoid& s2, const Transform3f& tf2,
+                                                           std::vector<ContactPoint>* contacts) const
+{
+  const bool res = details::ellipsoidHalfspaceIntersect(s2, tf2, s1, tf1, contacts);
+  if (contacts) flipNormal(*contacts);
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Halfspace>(const Box& s1, const Transform3f& tf1,
                                                      const Halfspace& s2, const Transform3f& tf2,
@@ -3226,6 +3368,24 @@ bool GJKSolver_indep::shapeIntersect<Plane, Sphere>(const Plane& s1, const Trans
   return res;
 }
 
+template<>
+bool GJKSolver_indep::shapeIntersect<Ellipsoid, Plane>(const Ellipsoid& s1, const Transform3f& tf1,
+                                                       const Plane& s2, const Transform3f& tf2,
+                                                       std::vector<ContactPoint>* contacts) const
+{
+  return details::ellipsoidPlaneIntersect(s1, tf1, s2, tf2, contacts);
+}
+
+template<>
+bool GJKSolver_indep::shapeIntersect<Plane, Ellipsoid>(const Plane& s1, const Transform3f& tf1,
+                                                       const Ellipsoid& s2, const Transform3f& tf2,
+                                                       std::vector<ContactPoint>* contacts) const
+{
+  const bool res = details::ellipsoidPlaneIntersect(s2, tf2, s1, tf1, contacts);
+  if (contacts) flipNormal(*contacts);
+  return res;
+}
+
 template<>
 bool GJKSolver_indep::shapeIntersect<Box, Plane>(const Box& s1, const Transform3f& tf1,
                                                  const Plane& s2, const Transform3f& tf2,
@@ -3339,25 +3499,27 @@ bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f&
 
 // Shape distance algorithms not using built-in GJK algorithm
 //
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// |            | box | sphere | capsule | cone | cylinder | plane | half-space | triangle |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | box        |     |        |         |      |          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | sphere     |/////|   O    |    O    |      |          |       |            |    O     |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | capsule    |/////|////////|    O    |      |          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cone       |/////|////////|/////////|      |          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | cylinder   |/////|////////|/////////|//////|          |       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | plane      |/////|////////|/////////|//////|//////////|       |            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | half-space |/////|////////|/////////|//////|//////////|///////|            |          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
-// | triangle   |/////|////////|/////////|//////|//////////|///////|////////////|          |
-// +------------+-----+--------+---------+------+----------+-------+------------+----------+
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |     |        |           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |    O    |      |          |       |            |     O    |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|    O    |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
 
 template<>
 bool GJKSolver_indep::shapeDistance<Sphere, Capsule>(const Sphere& s1, const Transform3f& tf1,
diff --git a/src/shape/geometric_shapes.cpp b/src/shape/geometric_shapes.cpp
index 5824f7c9e..9306d0a29 100644
--- a/src/shape/geometric_shapes.cpp
+++ b/src/shape/geometric_shapes.cpp
@@ -145,6 +145,13 @@ void Sphere::computeLocalAABB()
   aabb_radius = radius;
 }
 
+void Ellipsoid::computeLocalAABB()
+{
+  computeBV<AABB>(*this, Transform3f(), aabb_local);
+  aabb_center = aabb_local.center();
+  aabb_radius = (aabb_local.min_ - aabb_center).length();
+}
+
 void Capsule::computeLocalAABB()
 {
   computeBV<AABB>(*this, Transform3f(), aabb_local);
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 4c3772945..5ec616a78 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -69,7 +69,7 @@ std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf)
   std::vector<Vec3f> result(12);
   const FCL_REAL m = (1 + sqrt(5.0)) / 2.0;
   FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0));
-  
+
   FCL_REAL a = edge_size;
   FCL_REAL b = m * edge_size;
   result[0] = tf.transform(Vec3f(0, a, b));
@@ -88,6 +88,47 @@ std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf)
   return result;
 }
 
+std::vector<Vec3f> getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf)
+{
+  // we use scaled icosahedron to bound the ellipsoid
+
+  std::vector<Vec3f> result(12);
+
+  const FCL_REAL phi = (1.0 + sqrt(5.0)) / 2.0;  // golden ratio
+  // TODO: Replace with constexpr when we migrate to C++11 or
+  //       boost::math::constants::phi<FCL_REAL>() if boost version is greater
+  //       than 1.50.
+
+  const FCL_REAL a = std::sqrt(3.0) / (phi * phi);
+  const FCL_REAL b = phi * a;
+
+  const FCL_REAL& A = ellipsoid.radii[0];
+  const FCL_REAL& B = ellipsoid.radii[1];
+  const FCL_REAL& C = ellipsoid.radii[2];
+
+  const FCL_REAL Aa = A * a;
+  const FCL_REAL Ab = A * b;
+  const FCL_REAL Ba = B * a;
+  const FCL_REAL Bb = B * b;
+  const FCL_REAL Ca = C * a;
+  const FCL_REAL Cb = C * b;
+
+  result[0] = tf.transform(Vec3f(0, Ba, Cb));
+  result[1] = tf.transform(Vec3f(0, -Ba, Cb));
+  result[2] = tf.transform(Vec3f(0, Ba, -Cb));
+  result[3] = tf.transform(Vec3f(0, -Ba, -Cb));
+  result[4] = tf.transform(Vec3f(Aa, Bb, 0));
+  result[5] = tf.transform(Vec3f(-Aa, Bb, 0));
+  result[6] = tf.transform(Vec3f(Aa, -Bb, 0));
+  result[7] = tf.transform(Vec3f(-Aa, -Bb, 0));
+  result[8] = tf.transform(Vec3f(Ab, 0, Ca));
+  result[9] = tf.transform(Vec3f(Ab, 0, -Ca));
+  result[10] = tf.transform(Vec3f(-Ab, 0, Ca));
+  result[11] = tf.transform(Vec3f(-Ab, 0, -Ca));
+
+  return result;
+}
+
 std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf)
 {
   std::vector<Vec3f> result(36);
@@ -272,6 +313,21 @@ void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv)
   bv.min_ = T - v_delta;
 }
 
+template<>
+void computeBV<AABB, Ellipsoid>(const Ellipsoid& 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.radii[0]) + fabs(R(0, 1) * s.radii[1]) + fabs(R(0, 2) * s.radii[2]));
+  FCL_REAL y_range = (fabs(R(1, 0) * s.radii[0]) + fabs(R(1, 1) * s.radii[1]) + fabs(R(1, 2) * s.radii[2]));
+  FCL_REAL z_range = (fabs(R(2, 0) * s.radii[0]) + fabs(R(2, 1) * s.radii[1]) + fabs(R(2, 2) * s.radii[2]));
+
+  Vec3f v_delta(x_range, y_range, z_range);
+  bv.max_ = T + v_delta;
+  bv.min_ = T - v_delta;
+}
+
 template<>
 void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
 {
@@ -430,6 +486,19 @@ void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
   bv.extent.setValue(s.radius);
 }
 
+template<>
+void computeBV<OBB, Ellipsoid>(const Ellipsoid& s, const Transform3f& tf, OBB& bv)
+{
+  const Matrix3f& R = tf.getRotation();
+  const Vec3f& T = tf.getTranslation();
+
+  bv.To = T;
+  bv.axis[0] = R.getColumn(0);
+  bv.axis[1] = R.getColumn(1);
+  bv.axis[2] = R.getColumn(2);
+  bv.extent = s.radii;
+}
+
 template<>
 void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
 {
diff --git a/test/test_fcl_broadphase.cpp b/test/test_fcl_broadphase.cpp
index 91d24bb4a..85f3c6485 100644
--- a/test/test_fcl_broadphase.cpp
+++ b/test/test_fcl_broadphase.cpp
@@ -379,6 +379,19 @@ void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double
                                                             z * step_size + delta_size + 0.5 * single_size - env_scale))));
   }
 
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Ellipsoid* ellipsoid = new Ellipsoid(single_size / 2, single_size / 2, single_size / 2);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(ellipsoid),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
   for(; i < n_edge * n_edge * n_edge / 4; ++i)
   {
     int x = i % (n_edge * n_edge);
@@ -440,7 +453,22 @@ void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, do
     BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
     generateBVHModel(*model, sphere, Transform3f(), 16, 16);
     env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model),
-                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Ellipsoid ellipsoid(single_size / 2, single_size / 2, single_size / 2);
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, ellipsoid, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             y * step_size + delta_size + 0.5 * single_size - env_scale,
                                                             z * step_size + delta_size + 0.5 * single_size - env_scale))));
   }
diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp
index d791b205b..bc0a7fdd3 100644
--- a/test/test_fcl_collision.cpp
+++ b/test/test_fcl_collision.cpp
@@ -159,6 +159,15 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
       BOOST_CHECK(overlap_obb >= overlap_sphere);
     }
 
+    {
+      Ellipsoid ellipsoid(len, len, len);
+      computeBV(ellipsoid, transforms[i], obb2);
+
+      bool overlap_obb = obb1.overlap(obb2);
+      bool overlap_ellipsoid = solver.shapeIntersect(box1, box1_tf, ellipsoid, transforms[i], NULL, NULL, NULL);
+      BOOST_CHECK(overlap_obb >= overlap_ellipsoid);
+    }
+
     {
       Capsule capsule(len, 2 * len);
       computeBV(capsule, transforms[i], obb2);
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
old mode 100644
new mode 100755
index ba01f027f..70bca7155
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -55,6 +55,18 @@ GJKSolver_indep solver2;
 
 #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p))
 
+BOOST_AUTO_TEST_CASE(sphere_shape)
+{
+  const double tol = 1e-12;
+  const double radius = 5.0;
+  const double pi = boost::math::constants::pi<FCL_REAL>();
+
+  Sphere s(radius);
+
+  const double volume = 4.0 / 3.0 * pi * radius * radius * radius;
+  BOOST_CHECK_CLOSE(volume, s.computeVolume(), tol);
+}
+
 BOOST_AUTO_TEST_CASE(gjkcache)
 {
   Cylinder s1(5, 10);
@@ -462,6 +474,30 @@ void testShapeIntersection(
   }
 }
 
+// Shape intersection test coverage (libccd)
+//
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |   O    |           |         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |         |      |          |   O   |      O     |    O     |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|     O     |         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|         |      |          |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|  O   |    O     |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|    O     |   O   |      O     |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+
 BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere)
 {
   Sphere s1(20);
@@ -977,6 +1013,75 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercone)
   testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
 }
 
+BOOST_AUTO_TEST_CASE(shapeIntersection_ellipsoidellipsoid)
+{
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
+
+  Transform3f tf1;
+  Transform3f tf2;
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  std::vector<ContactPoint> contacts;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(40, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(30, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(29.99, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-29.99, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-29.99, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-30, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
+}
+
 BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
 {
   Sphere s(10);
@@ -1318,122 +1423,578 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox)
   tf1 = transform;
   tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
   contacts.resize(1);
-  contacts[0].pos = transform.transform(Vec3f(-1.875, 0, 0));
-  contacts[0].penetration_depth = 1.25;
-  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  contacts[0].pos = transform.transform(Vec3f(-1.875, 0, 0));
+  contacts[0].penetration_depth = 1.25;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0.005, 0, 0);
+  contacts[0].penetration_depth = 5.01;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0.005, 0, 0));
+  contacts[0].penetration_depth = 5.01;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f(transform.getQuatRotation());
+  tf2 = Transform3f();
+  contacts.resize(1);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
+}
+
+BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
+{
+  Box s(5, 10, 20);
+  Plane hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f tf1;
+  Transform3f tf2;
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  std::vector<ContactPoint> contacts;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0, 0);
+  contacts[0].penetration_depth = 2.5;
+  contacts[0].normal.setValue(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, 0));
+  contacts[0].penetration_depth = 2.5;
+  contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(1.25, 0, 0);
+  contacts[0].penetration_depth = 1.25;
+  contacts[0].normal.setValue(1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(1.25, 0, 0));
+  contacts[0].penetration_depth = 1.25;
+  contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(-1.25, 0, 0);
+  contacts[0].penetration_depth = 1.25;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0));
+  contacts[0].penetration_depth = 1.25;
+  contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f(transform.getQuatRotation());
+  tf2 = Transform3f();
+  contacts.resize(1);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
+}
+
+BOOST_AUTO_TEST_CASE(shapeIntersection_halfspaceellipsoid)
+{
+  Ellipsoid s(5, 10, 20);
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f tf1;
+  Transform3f tf2;
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  std::vector<ContactPoint> contacts;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  contacts[0].pos.setValue(-2.5, 0, 0);
+  contacts[0].penetration_depth = 5.0;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0));
+  contacts[0].penetration_depth = 5.0;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(-1.875, 0, 0);
+  contacts[0].penetration_depth = 6.25;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(-1.875, 0, 0));
+  contacts[0].penetration_depth = 6.25;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(-3.125, 0, 0);
+  contacts[0].penetration_depth = 3.75;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(-3.125, 0, 0));
+  contacts[0].penetration_depth = 3.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.01, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0.005, 0, 0);
+  contacts[0].penetration_depth = 10.01;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(5.01, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0.005, 0, 0));
+  contacts[0].penetration_depth = 10.01;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-5.01, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-5.01, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 1, 0), 0);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, -5.0, 0);
+  contacts[0].penetration_depth = 10.0;
+  contacts[0].normal.setValue(0, -1, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, -5.0, 0));
+  contacts[0].penetration_depth = 10.0;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 1.25, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, -4.375, 0);
+  contacts[0].penetration_depth = 11.25;
+  contacts[0].normal.setValue(0, -1, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 1.25, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, -4.375, 0));
+  contacts[0].penetration_depth = 11.25;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -1.25, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, -5.625, 0);
+  contacts[0].penetration_depth = 8.75;
+  contacts[0].normal.setValue(0, -1, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -1.25, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, -5.625, 0));
+  contacts[0].penetration_depth = 8.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 10.01, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0.005, 0);
+  contacts[0].penetration_depth = 20.01;
+  contacts[0].normal.setValue(0, -1, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 10.01, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0.005, 0));
+  contacts[0].penetration_depth = 20.01;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -10.01, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -10.01, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 0, 1), 0);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0, -10.0);
+  contacts[0].penetration_depth = 20.0;
+  contacts[0].normal.setValue(0, 0, -1);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, -10.0));
+  contacts[0].penetration_depth = 20.0;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 1.25));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0, -9.375);
+  contacts[0].penetration_depth = 21.25;
+  contacts[0].normal.setValue(0, 0, -1);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 1.25));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, -9.375));
+  contacts[0].penetration_depth = 21.25;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -1.25));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0, -10.625);
+  contacts[0].penetration_depth = 18.75;
+  contacts[0].normal.setValue(0, 0, -1);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -1.25));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, -10.625));
+  contacts[0].penetration_depth = 18.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, 20.01));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0, 0.005);
+  contacts[0].penetration_depth = 40.01;
+  contacts[0].normal.setValue(0, 0, -1);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, 20.01));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, 0.005));
+  contacts[0].penetration_depth = 40.01;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 0, -20.01));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 0, -20.01));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+}
+
+BOOST_AUTO_TEST_CASE(shapeIntersection_planeellipsoid)
+{
+  Ellipsoid s(5, 10, 20);
+  Plane hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f tf1;
+  Transform3f tf2;
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  std::vector<ContactPoint> contacts;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0, 0);
+  contacts[0].penetration_depth = 5.0;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, 0));
+  contacts[0].penetration_depth = 5.0;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(1.25, 0, 0);
+  contacts[0].penetration_depth = 3.75;
+  contacts[0].normal.setValue(1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(1.25, 0, 0));
+  contacts[0].penetration_depth = 3.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(-1.25, 0, 0);
+  contacts[0].penetration_depth = 3.75;
+  contacts[0].normal.setValue(-1, 0, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0));
+  contacts[0].penetration_depth = 3.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(5.01, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(5.01, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-5.01, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-5.01, 0, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
+
+
+
+
+  hs = Plane(Vec3f(0, 1, 0), 0);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 0.0, 0);
+  contacts[0].penetration_depth = 10.0;
+  contacts[0].normal.setValue(0, -1, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, 0));
+  contacts[0].penetration_depth = 10.0;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, 1.25, 0));
+  contacts.resize(1);
+  contacts[0].pos.setValue(0, 1.25, 0);
+  contacts[0].penetration_depth = 8.75;
+  contacts[0].normal.setValue(0, 1, 0);
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, 1.25, 0));
+  contacts.resize(1);
+  contacts[0].pos = transform.transform(Vec3f(0, 1.25, 0));
+  contacts[0].penetration_depth = 8.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 1, 0));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
 
   tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  tf2 = Transform3f(Vec3f(0, -1.25, 0));
   contacts.resize(1);
-  contacts[0].pos.setValue(0.005, 0, 0);
-  contacts[0].penetration_depth = 5.01;
-  contacts[0].normal.setValue(-1, 0, 0);
+  contacts[0].pos.setValue(0, -1.25, 0);
+  contacts[0].penetration_depth = 8.75;
+  contacts[0].normal.setValue(0, -1, 0);
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+  tf2 = transform * Transform3f(Vec3f(0, -1.25, 0));
   contacts.resize(1);
-  contacts[0].pos = transform.transform(Vec3f(0.005, 0, 0));
-  contacts[0].penetration_depth = 5.01;
-  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0));
+  contacts[0].pos = transform.transform(Vec3f(0, -1.25, 0));
+  contacts[0].penetration_depth = 8.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
 
   tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  tf2 = Transform3f(Vec3f(0, 10.01, 0));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  tf2 = transform * Transform3f(Vec3f(0, 10.01, 0));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  tf1 = Transform3f(transform.getQuatRotation());
-  tf2 = Transform3f();
-  contacts.resize(1);
-  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
-}
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(0, -10.01, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-BOOST_AUTO_TEST_CASE(shapeIntersection_planebox)
-{
-  Box s(5, 10, 20);
-  Plane hs(Vec3f(1, 0, 0), 0);
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(0, -10.01, 0));
+  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
-  Transform3f tf1;
-  Transform3f tf2;
 
-  Transform3f transform;
-  generateRandomTransform(extents, transform);
 
-  std::vector<ContactPoint> contacts;
+
+  hs = Plane(Vec3f(0, 0, 1), 0);
 
   tf1 = Transform3f();
   tf2 = Transform3f();
   contacts.resize(1);
   contacts[0].pos.setValue(0, 0, 0);
-  contacts[0].penetration_depth = 2.5;
-  contacts[0].normal.setValue(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  contacts[0].penetration_depth = 20.0;
+  contacts[0].normal.setValue(0, 0, -1);
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
 
   tf1 = transform;
   tf2 = transform;
   contacts.resize(1);
   contacts[0].pos = transform.transform(Vec3f(0, 0, 0));
-  contacts[0].penetration_depth = 2.5;
-  contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
+  contacts[0].penetration_depth = 20.0;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);
 
   tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(1.25, 0, 0));
+  tf2 = Transform3f(Vec3f(0, 0, 1.25));
   contacts.resize(1);
-  contacts[0].pos.setValue(1.25, 0, 0);
-  contacts[0].penetration_depth = 1.25;
-  contacts[0].normal.setValue(1, 0, 0);
+  contacts[0].pos.setValue(0, 0, 1.25);
+  contacts[0].penetration_depth = 18.75;
+  contacts[0].normal.setValue(0, 0, 1);
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(1.25, 0, 0));
+  tf2 = transform * Transform3f(Vec3f(0, 0, 1.25));
   contacts.resize(1);
-  contacts[0].pos = transform.transform(Vec3f(1.25, 0, 0));
-  contacts[0].penetration_depth = 1.25;
-  contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, 1.25));
+  contacts[0].penetration_depth = 18.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, 1));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
 
   tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-1.25, 0, 0));
+  tf2 = Transform3f(Vec3f(0, 0, -1.25));
   contacts.resize(1);
-  contacts[0].pos.setValue(-1.25, 0, 0);
-  contacts[0].penetration_depth = 1.25;
-  contacts[0].normal.setValue(-1, 0, 0);
+  contacts[0].pos.setValue(0, 0, -1.25);
+  contacts[0].penetration_depth = 18.75;
+  contacts[0].normal.setValue(0, 0, -1);
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0));
+  tf2 = transform * Transform3f(Vec3f(0, 0, -1.25));
   contacts.resize(1);
-  contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0));
-  contacts[0].penetration_depth = 1.25;
-  contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0);
+  contacts[0].pos = transform.transform(Vec3f(0, 0, -1.25));
+  contacts[0].penetration_depth = 18.75;
+  contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
 
   tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(2.51, 0, 0));
+  tf2 = Transform3f(Vec3f(0, 0, 20.01));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(2.51, 0, 0));
+  tf2 = transform * Transform3f(Vec3f(0, 0, 20.01));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
   tf1 = Transform3f();
-  tf2 = Transform3f(Vec3f(-2.51, 0, 0));
+  tf2 = Transform3f(Vec3f(0, 0, -20.01));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 
   tf1 = transform;
-  tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0));
+  tf2 = transform * Transform3f(Vec3f(0, 0, -20.01));
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
-
-  tf1 = Transform3f(transform.getQuatRotation());
-  tf2 = Transform3f();
-  contacts.resize(1);
-  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
 }
 
 BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule)
@@ -2805,7 +3366,29 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planecone)
   testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
 }
 
-
+// Shape distance test coverage (libccd)
+//
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |   O    |           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|     O     |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|  O   |    O     |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|    O     |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
 
 BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere)
 {
@@ -3007,8 +3590,6 @@ BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder)
   BOOST_CHECK(res);
 }
 
-
-
 BOOST_AUTO_TEST_CASE(shapeDistance_conecone)
 {
   Cone s1(5, 10);
@@ -3081,7 +3662,91 @@ BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder)
   BOOST_CHECK(res);
 }
 
+BOOST_AUTO_TEST_CASE(shapeDistance_ellipsoidellipsoid)
+{
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist = -1;
+  Vec3f closest_p1, closest_p2;
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist, &closest_p1, &closest_p2);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+}
 
+// Shape intersection test coverage (built-in GJK)
+//
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |   O    |           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |         |      |          |       |            |     O    |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|     O     |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|  O   |    O     |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|    O     |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere)
 {
@@ -3527,6 +4192,76 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercone)
   testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
 }
 
+BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_ellipsoidellipsoid)
+{
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
+
+  Transform3f tf1;
+  Transform3f tf2;
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  std::vector<ContactPoint> contacts;
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(40, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(40, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(30, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(30.01, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(29.99, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(29.9, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f();
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform;
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-29.99, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-29.99, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = Transform3f();
+  tf2 = Transform3f(Vec3f(-30, 0, 0));
+  contacts.resize(1);
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
+
+  tf1 = transform;
+  tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0));
+  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
+}
 
 BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
 {
@@ -3592,8 +4327,8 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle)
 
 
   t[0].setValue(20, 0, 0);
-  t[1].setValue(0, -20, 0);
-  t[2].setValue(0, 20, 0);
+  t[1].setValue(-0.1, -20, 0);
+  t[2].setValue(-0.1, 20, 0);
   res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL);
   BOOST_CHECK(res);
 
@@ -3650,8 +4385,29 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle)
   BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
 }
 
-
-
+// Shape distance test coverage (built-in GJK)
+//
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | box        |  O  |   O    |           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | sphere     |/////|   O    |           |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | ellipsoid  |/////|////////|     O     |         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | capsule    |/////|////////|///////////|         |      |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cone       |/////|////////|///////////|/////////|  O   |          |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | cylinder   |/////|////////|///////////|/////////|//////|    O     |       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
+// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
+// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
 
 BOOST_AUTO_TEST_CASE(shapeDistanceGJK_spheresphere)
 {
@@ -3714,7 +4470,7 @@ BOOST_AUTO_TEST_CASE(shapeDistanceGJK_spheresphere)
   BOOST_CHECK_FALSE(res);
 }
 
-BOOST_AUTO_TEST_CASE(hapeDistanceGJK_boxbox)
+BOOST_AUTO_TEST_CASE(shapeDistanceGJK_boxbox)
 {
   Box s1(20, 40, 50);
   Box s2(10, 10, 10);
@@ -3750,7 +4506,7 @@ BOOST_AUTO_TEST_CASE(hapeDistanceGJK_boxbox)
   BOOST_CHECK(res);
 }
 
-BOOST_AUTO_TEST_CASE(hapeDistanceGJK_boxsphere)
+BOOST_AUTO_TEST_CASE(shapeDistanceGJK_boxsphere)
 {
   Sphere s1(20);
   Box s2(5, 5, 5);
@@ -3786,7 +4542,7 @@ BOOST_AUTO_TEST_CASE(hapeDistanceGJK_boxsphere)
   BOOST_CHECK(res);
 }
 
-BOOST_AUTO_TEST_CASE(hapeDistanceGJK_cylindercylinder)
+BOOST_AUTO_TEST_CASE(shapeDistanceGJK_cylindercylinder)
 {
   Cylinder s1(5, 10);
   Cylinder s2(5, 10);
@@ -3822,9 +4578,7 @@ BOOST_AUTO_TEST_CASE(hapeDistanceGJK_cylindercylinder)
   BOOST_CHECK(res);
 }
 
-
-
-BOOST_AUTO_TEST_CASE(hapeDistanceGJK_conecone)
+BOOST_AUTO_TEST_CASE(shapeDistanceGJK_conecone)
 {
   Cone s1(5, 10);
   Cone s2(5, 10);
@@ -3860,8 +4614,65 @@ BOOST_AUTO_TEST_CASE(hapeDistanceGJK_conecone)
   BOOST_CHECK(res);
 }
 
+BOOST_AUTO_TEST_CASE(shapeDistanceGJK_ellipsoidellipsoid)
+{
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist = -1;
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
+  BOOST_CHECK(fabs(dist - 10) < 0.001);
+  BOOST_CHECK(res);
 
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
+  BOOST_CHECK(fabs(dist - 0.1) < 0.001);
+  BOOST_CHECK(res);
 
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
+  BOOST_CHECK(dist < 0);
+  BOOST_CHECK_FALSE(res);
+}
 
 template<typename S1, typename S2>
 void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance)
@@ -3910,9 +4721,10 @@ BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes)
   // reverse case as well. For example, if FCL has sphere-capsule intersection
   // algorithm, then this algorithm should be called for capsule-sphere case.
 
-  // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+  // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace
   Box box(10, 10, 10);
   Sphere sphere(5);
+  Ellipsoid ellipsoid(5, 5, 5);
   Capsule capsule(5, 10);
   Cone cone(5, 10);
   Cylinder cylinder(5, 10);
@@ -3927,18 +4739,26 @@ BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes)
   // algorithm is added, then uncomment box-sphere.
 
 //  testReversibleShapeIntersection(box, sphere, distance);
+//  testReversibleShapeIntersection(box, ellipsoid, distance);
 //  testReversibleShapeIntersection(box, capsule, distance);
 //  testReversibleShapeIntersection(box, cone, distance);
 //  testReversibleShapeIntersection(box, cylinder, distance);
   testReversibleShapeIntersection(box, plane, distance);
   testReversibleShapeIntersection(box, halfspace, distance);
 
+//  testReversibleShapeIntersection(sphere, ellipsoid, distance);
   testReversibleShapeIntersection(sphere, capsule, distance);
 //  testReversibleShapeIntersection(sphere, cone, distance);
 //  testReversibleShapeIntersection(sphere, cylinder, distance);
   testReversibleShapeIntersection(sphere, plane, distance);
   testReversibleShapeIntersection(sphere, halfspace, distance);
 
+//  testReversibleShapeIntersection(ellipsoid, capsule, distance);
+//  testReversibleShapeIntersection(ellipsoid, cone, distance);
+//  testReversibleShapeIntersection(ellipsoid, cylinder, distance);
+//  testReversibleShapeIntersection(ellipsoid, plane, distance);
+//  testReversibleShapeIntersection(ellipsoid, halfspace, distance);
+
 //  testReversibleShapeIntersection(capsule, cone, distance);
 //  testReversibleShapeIntersection(capsule, cylinder, distance);
   testReversibleShapeIntersection(capsule, plane, distance);
@@ -3997,9 +4817,10 @@ BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
   // reverse case as well. For example, if FCL has sphere-capsule distance
   // algorithm, then this algorithm should be called for capsule-sphere case.
 
-  // Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
+  // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace
   Box box(10, 10, 10);
   Sphere sphere(5);
+  Ellipsoid ellipsoid(5, 5, 5);
   Capsule capsule(5, 10);
   Cone cone(5, 10);
   Cylinder cylinder(5, 10);
@@ -4014,18 +4835,26 @@ BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
   // algorithm is added, then uncomment box-sphere.
 
 //  testReversibleShapeDistance(box, sphere, distance);
+//  testReversibleShapeDistance(box, ellipsoid, distance);
 //  testReversibleShapeDistance(box, capsule, distance);
 //  testReversibleShapeDistance(box, cone, distance);
 //  testReversibleShapeDistance(box, cylinder, distance);
 //  testReversibleShapeDistance(box, plane, distance);
 //  testReversibleShapeDistance(box, halfspace, distance);
 
+//  testReversibleShapeDistance(sphere, ellipsoid, distance);
   testReversibleShapeDistance(sphere, capsule, distance);
 //  testReversibleShapeDistance(sphere, cone, distance);
 //  testReversibleShapeDistance(sphere, cylinder, distance);
 //  testReversibleShapeDistance(sphere, plane, distance);
 //  testReversibleShapeDistance(sphere, halfspace, distance);
 
+//  testReversibleShapeDistance(ellipsoid, capsule, distance);
+//  testReversibleShapeDistance(ellipsoid, cone, distance);
+//  testReversibleShapeDistance(ellipsoid, cylinder, distance);
+//  testReversibleShapeDistance(ellipsoid, plane, distance);
+//  testReversibleShapeDistance(ellipsoid, halfspace, distance);
+
 //  testReversibleShapeDistance(capsule, cone, distance);
 //  testReversibleShapeDistance(capsule, cylinder, distance);
 //  testReversibleShapeDistance(capsule, plane, distance);
diff --git a/test/test_fcl_shape_mesh_consistency.cpp b/test/test_fcl_shape_mesh_consistency.cpp
index 552f3fe1c..9b7dcebec 100644
--- a/test/test_fcl_shape_mesh_consistency.cpp
+++ b/test/test_fcl_shape_mesh_consistency.cpp
@@ -142,6 +142,96 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd)
   }
 }
 
+BOOST_AUTO_TEST_CASE(consistency_distance_ellipsoidellipsoid_libccd)
+{
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(40, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  }
+
+  pose.setTranslation(Vec3f(30.1, 0, 0));
+
+  res.clear(), res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
 BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd)
 {
   Box s1(20, 40, 50);
@@ -508,6 +598,99 @@ BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK)
   }
 }
 
+BOOST_AUTO_TEST_CASE(consistency_distance_ellipsoidellipsoid_GJK)
+{
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+
+  DistanceRequest request;
+  request.gjk_solver_type = GST_INDEP;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(40, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  }
+
+  pose.setTranslation(Vec3f(30.1, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, request, res1);
+  BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
+
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, request, res1);
+    BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
+  }
+}
+
 BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK)
 {
   Box s1(20, 40, 50);
@@ -520,6 +703,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK)
   generateBVHModel(s2_rss, s2, Transform3f());
 
   DistanceRequest request;
+  request.gjk_solver_type = GST_INDEP;
   DistanceResult res, res1;
 
   Transform3f pose;
@@ -611,6 +795,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK)
   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
   
   DistanceRequest request;
+  request.gjk_solver_type = GST_INDEP;
   DistanceResult res, res1;
 
   Transform3f pose;
@@ -711,6 +896,7 @@ BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK)
   generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
 
   DistanceRequest request;
+  request.gjk_solver_type = GST_INDEP;
   DistanceResult res, res1;
 
   Transform3f pose;
@@ -878,7 +1064,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
   result.clear();
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
-  
+
   pose.setTranslation(Vec3f(30, 0, 0));
   pose_aabb.setTranslation(Vec3f(30, 0, 0));
   pose_obb.setTranslation(Vec3f(30, 0, 0));
@@ -910,7 +1096,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
   result.clear();
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
-  
+
   pose.setTranslation(Vec3f(29.9, 0, 0));
   pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
   pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
@@ -1009,20 +1195,19 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd)
   BOOST_CHECK_FALSE(res);
 }
 
-BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
+BOOST_AUTO_TEST_CASE(consistency_collision_ellipsoidellipsoid_libccd)
 {
-  Box s1(20, 40, 50);
-  Box s2(10, 10, 10);
-
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
   BVHModel<AABB> s1_aabb;
   BVHModel<AABB> s2_aabb;
   BVHModel<OBB> s1_obb;
   BVHModel<OBB> s2_obb;
 
-  generateBVHModel(s1_aabb, s1, Transform3f());
-  generateBVHModel(s2_aabb, s2, Transform3f());
-  generateBVHModel(s1_obb, s1, Transform3f());
-  generateBVHModel(s2_obb, s2, Transform3f());
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
 
   CollisionRequest request;
   CollisionResult result;
@@ -1031,6 +1216,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
 
   Transform3f pose, pose_aabb, pose_obb;
 
+
   // s2 is within s1
   // both are shapes --> collision
   // s1 is shape, s2 is mesh --> in collision
@@ -1064,9 +1250,9 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(15.01, 0, 0));
-  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
-  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+  pose.setTranslation(Vec3f(40, 0, 0));
+  pose_aabb.setTranslation(Vec3f(40, 0, 0));
+  pose_obb.setTranslation(Vec3f(40, 0, 0));
 
   result.clear();
   res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
@@ -1096,66 +1282,45 @@ BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
 
-  pose.setTranslation(Vec3f(14.99, 0, 0));
-  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
-  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+  pose.setTranslation(Vec3f(30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(30, 0, 0));
+  pose_obb.setTranslation(Vec3f(30, 0, 0));
 
   result.clear();
   res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);  // libccd cannot detect collision when two ellipsoid is exactly touching each other
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.999, 0, 0)), request, result) > 0);
   BOOST_CHECK(res);
   result.clear();
   res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
-  BOOST_CHECK(res);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
-  BOOST_CHECK(res);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
-  BOOST_CHECK(res);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
-  BOOST_CHECK(res);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
-  BOOST_CHECK(res);
-}
-
-BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
-{
-  Sphere s1(20);
-  Box s2(5, 5, 5);
-
-  BVHModel<AABB> s1_aabb;
-  BVHModel<AABB> s2_aabb;
-  BVHModel<OBB> s1_obb;
-  BVHModel<OBB> s2_obb;
-
-  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_aabb, s2, Transform3f());
-  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
-  generateBVHModel(s2_obb, s2, Transform3f());
-
-  CollisionRequest request;
-  CollisionResult result;
-
-  bool res;
+  BOOST_CHECK_FALSE(res);
 
-  Transform3f pose, pose_aabb, pose_obb;
+  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(29.9, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(29.9, 0, 0)); // 29.9 fails, result depends on mesh precision
 
-  // s2 is within s1
-  // both are shapes --> collision
-  // s1 is shape, s2 is mesh --> in collision
-  // s1 is mesh, s2 is shape --> collision free
-  // all are reasonable
   result.clear();
   res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK(res);
@@ -1173,10 +1338,250 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
   BOOST_CHECK(res);
   result.clear();
   res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  BOOST_CHECK(res);
   result.clear();
   res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
-  BOOST_CHECK_FALSE(res);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+
+  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_obb.setTranslation(Vec3f(-29.9, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+
+  pose.setTranslation(Vec3f(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
+  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+}
+
+BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f());
+  generateBVHModel(s2_aabb, s2, Transform3f());
+  generateBVHModel(s1_obb, s1, Transform3f());
+  generateBVHModel(s2_obb, s2, Transform3f());
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+
+  pose.setTranslation(Vec3f(15.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+
+  pose.setTranslation(Vec3f(14.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+}
+
+BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f());
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f());
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
   result.clear();
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
@@ -1578,7 +1983,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
   result.clear();
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
-  
+
   pose.setTranslation(Vec3f(30, 0, 0));
   pose_aabb.setTranslation(Vec3f(30, 0, 0));
   pose_obb.setTranslation(Vec3f(30, 0, 0));
@@ -1610,7 +2015,7 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
   result.clear();
   res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
   BOOST_CHECK_FALSE(res);
-  
+
   pose.setTranslation(Vec3f(29.9, 0, 0));
   pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
   pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
@@ -1709,6 +2114,225 @@ BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK)
   BOOST_CHECK_FALSE(res);
 }
 
+BOOST_AUTO_TEST_CASE(consistency_collision_ellipsoidellipsoid_GJK)
+{
+  Ellipsoid s1(20, 40, 50);
+  Ellipsoid s2(10, 10, 10);
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
+
+  CollisionRequest request;
+  request.gjk_solver_type = GST_INDEP;
+
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+
+  pose.setTranslation(Vec3f(40, 0, 0));
+  pose_aabb.setTranslation(Vec3f(40, 0, 0));
+  pose_obb.setTranslation(Vec3f(40, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+
+  pose.setTranslation(Vec3f(30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(30, 0, 0));
+  pose_obb.setTranslation(Vec3f(30, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+
+  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(29.9, 0, 0));
+  pose_obb.setTranslation(Vec3f(29.9, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+
+
+  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_obb.setTranslation(Vec3f(-29.9, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+
+  pose.setTranslation(Vec3f(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
+  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0);
+  BOOST_CHECK_FALSE(res);
+}
+
 BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK)
 {
   Box s1(20, 40, 50);
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index 76f8e952f..07808ab37 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -424,6 +424,8 @@ std::string getNodeTypeName(NODE_TYPE node_type)
     return std::string("GEOM_BOX");
   else if (node_type == GEOM_SPHERE)
     return std::string("GEOM_SPHERE");
+  else if (node_type == GEOM_ELLIPSOID)
+    return std::string("GEOM_ELLIPSOID");
   else if (node_type == GEOM_CAPSULE)
     return std::string("GEOM_CAPSULE");
   else if (node_type == GEOM_CONE)