diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 972f491338036b3ef5e4e30f88d110c471ee2f02..19f3955a78eaee2fe6a0f49d73031e4609edb7f6 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -90,7 +90,15 @@ install(FILES "${pkg_conf_file}" DESTINATION lib/pkgconfig/ COMPONENT pkgconfig)
 enable_testing()
 find_package(GTest REQUIRED)
 include_directories(${GTEST_INCLUDE_DIRS})
+
 add_executable(test_fcl_collision test/test_fcl_collision.cpp test/test_fcl_utility.cpp)
 target_link_libraries(test_fcl_collision ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
-
 GTEST_ADD_TESTS(test_fcl_collision "" test/test_fcl_collision.cpp)
+
+add_executable(test_fcl_distance test/test_fcl_distance.cpp test/test_fcl_utility.cpp)
+target_link_libraries(test_fcl_distance ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
+GTEST_ADD_TESTS(test_fcl_distance "" test/test_fcl_distance.cpp)
+
+add_executable(test_fcl_geometric_shapes test/test_fcl_geometric_shapes.cpp test/test_fcl_utility.cpp)
+target_link_libraries(test_fcl_geometric_shapes ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
+GTEST_ADD_TESTS(test_fcl_geometric_shapes "" test/test_fcl_geometric_shapes.cpp)
diff --git a/trunk/fcl/test/test_fcl_distance.cpp b/trunk/fcl/test/test_fcl_distance.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..295412fb9819f13d5cc07771c29e92b85177c33c
--- /dev/null
+++ b/trunk/fcl/test/test_fcl_distance.cpp
@@ -0,0 +1,431 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#include "fcl/traversal/traversal_node_bvhs.h"
+#include "fcl/traversal/traversal_node_setup.h"
+#include "fcl/collision_node.h"
+#include "test_fcl_utility.h"
+#include <gtest/gtest.h>
+#include <boost/timer.hpp>
+
+using namespace fcl;
+
+bool verbose = false;
+FCL_REAL DELTA = 0.001;
+
+
+template<typename BV>
+void distance_Test(const Transform3f& tf,
+                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
+                   int qsize,
+                   DistanceRes& distance_result,
+                   bool verbose = true);
+
+bool collide_Test_OBB(const Transform3f& tf,
+                      const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                      const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose);
+
+
+template<typename BV, typename TraversalNode>
+void distance_Test_Oriented(const Transform3f& tf,
+                            const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                            const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
+                            int qsize,
+                            DistanceRes& distance_result,
+                            bool verbose = true);
+
+inline bool nearlyEqual(const Vec3f& a, const Vec3f& b)
+{
+  if(fabs(a[0] - b[0]) > DELTA) return false;
+  if(fabs(a[1] - b[1]) > DELTA) return false;
+  if(fabs(a[2] - b[2]) > DELTA) return false;
+  return true;
+}
+
+
+TEST(collision_core, mesh_distance)
+{
+  std::vector<Vec3f> p1, p2;
+  std::vector<Triangle> t1, t2;
+  loadOBJFile("../test/env.obj", p1, t1);
+  loadOBJFile("../test/rob.obj", p2, t2);
+
+  std::vector<Transform3f> transforms; // t0
+  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  std::size_t n = 10;
+
+  generateRandomTransforms(extents, transforms, n);
+
+  double dis_time = 0;
+  double col_time = 0;
+
+  DistanceRes res, res_now;
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    boost::timer timer_col;
+    collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    col_time += timer_col.elapsed();
+
+    boost::timer timer_dist;
+    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose);
+    dis_time += timer_dist.elapsed();
+
+    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+    
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+
+
+    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<kIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+
+    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+    
+    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+    distance_Test<OBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+
+    ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
+    ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+  }
+
+  std::cout << "distance timing: " << dis_time << " sec" << std::endl;
+  std::cout << "collision timing: " << col_time << " sec" << std::endl;
+}
+
+
+int main(int argc, char **argv)
+{
+  srand(time(NULL));
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+template<typename BV, typename TraversalNode>
+void distance_Test_Oriented(const Transform3f& tf,
+                            const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                            const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
+                            int qsize,
+                            DistanceRes& distance_result,
+                            bool verbose)
+{
+  BVHModel<BV> m1;
+  BVHModel<BV> m2;
+  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
+  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
+
+
+  m1.beginModel();
+  m1.addSubModel(vertices1, triangles1);
+  m1.endModel();
+
+  m2.beginModel();
+  m2.addSubModel(vertices2, triangles2);
+  m2.endModel();
+
+  DistanceResult local_result;
+  TraversalNode node;
+  if(!initialize(node, (const BVHModel<BV>&)m1, tf, (const BVHModel<BV>&)m2, Transform3f(), DistanceRequest(true), local_result))
+    std::cout << "initialize error" << std::endl;
+
+  node.enable_statistics = verbose;
+
+  distance(&node, NULL, qsize);
+
+  // points are in local coordinate, to global coordinate
+  Vec3f p1 = local_result.nearest_points[0];
+  Vec3f p2 = local_result.nearest_points[1];
+
+
+  distance_result.distance = local_result.min_distance;
+  distance_result.p1 = p1;
+  distance_result.p2 = p2;
+
+  if(verbose)
+  {
+    std::cout << "distance " << local_result.min_distance << std::endl;
+
+    std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl;
+    std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl;
+    std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
+  }
+}
+
+template<typename BV>
+void distance_Test(const Transform3f& tf,
+                   const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                   const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method,
+                   int qsize,
+                   DistanceRes& distance_result,
+                   bool verbose)
+{
+  BVHModel<BV> m1;
+  BVHModel<BV> m2;
+  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
+  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
+
+
+  m1.beginModel();
+  m1.addSubModel(vertices1, triangles1);
+  m1.endModel();
+
+  m2.beginModel();
+  m2.addSubModel(vertices2, triangles2);
+  m2.endModel();
+
+  Transform3f pose1(tf), pose2;
+
+  DistanceResult local_result;
+  MeshDistanceTraversalNode<BV> node;
+
+  if(!initialize<BV>(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result))
+    std::cout << "initialize error" << std::endl;
+
+  node.enable_statistics = verbose;
+
+  distance(&node, NULL, qsize);
+
+  distance_result.distance = local_result.min_distance;
+  distance_result.p1 = local_result.nearest_points[0];
+  distance_result.p2 = local_result.nearest_points[1];
+
+  if(verbose)
+  {
+    std::cout << "distance " << local_result.min_distance << std::endl;
+
+    std::cout << local_result.nearest_points[0][0] << " " << local_result.nearest_points[0][1] << " " << local_result.nearest_points[0][2] << std::endl;
+    std::cout << local_result.nearest_points[1][0] << " " << local_result.nearest_points[1][1] << " " << local_result.nearest_points[1][2] << std::endl;
+    std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
+  }
+}
+
+
+bool collide_Test_OBB(const Transform3f& tf,
+                      const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                      const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
+{
+  BVHModel<OBB> m1;
+  BVHModel<OBB> m2;
+  m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
+  m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
+
+  m1.beginModel();
+  m1.addSubModel(vertices1, triangles1);
+  m1.endModel();
+
+  m2.beginModel();
+  m2.addSubModel(vertices2, triangles2);
+  m2.endModel();
+
+  CollisionResult local_result;	
+  MeshCollisionTraversalNodeOBB node;
+  if(!initialize(node, (const BVHModel<OBB>&)m1, tf, (const BVHModel<OBB>&)m2, Transform3f(),
+                 CollisionRequest(), local_result))
+    std::cout << "initialize error" << std::endl;
+
+  node.enable_statistics = verbose;
+
+  collide(&node);
+
+  if(local_result.numContacts() > 0)
+    return true;
+  else
+    return false;
+}
+
+
diff --git a/trunk/fcl/test/test_fcl_geometric_shapes.cpp b/trunk/fcl/test/test_fcl_geometric_shapes.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cd3559aa844c7b67d0e8e93832b79c7fb920eb2d
--- /dev/null
+++ b/trunk/fcl/test/test_fcl_geometric_shapes.cpp
@@ -0,0 +1,2696 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#include "fcl/narrowphase/narrowphase.h"
+#include "fcl/collision.h"
+#include "test_fcl_utility.h"
+#include <gtest/gtest.h>
+#include <iostream>
+
+using namespace fcl;
+
+FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10};
+
+GJKSolver_libccd solver1;
+GJKSolver_indep solver2;
+
+TEST(shapeIntersection, spheresphere)
+{
+  Sphere s1(20);
+  Sphere s2(10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  CollisionRequest request;
+  CollisionResult result;
+  bool res;
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, boxbox)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(15, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  Quaternion3f q;
+  q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(q), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(q), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersection, spherebox)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.5, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.501, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.501, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(22.4, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersection, cylindercylinder)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.01, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, conecone)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10.001, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.001, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.001, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersection, conecylinder)
+{
+  Cylinder s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(10, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10, 0, 0)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(0, 0, 10.01)), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, spheretriangle)
+{
+  Sphere s(10);
+  Vec3f t[3];
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-20, 0, 0);
+  t[2].setValue(0, 20, 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+  Transform3f identity;
+
+  bool res;
+
+  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+
+  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+
+
+  t[0].setValue(30, 0, 0);
+  t[1].setValue(9.9, -20, 0);
+  t[2].setValue(9.9, 20, 0);
+  res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+
+  res =  solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersection, halfspacesphere)
+{
+  Sphere s(10);
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 15) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-2.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 15) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-7.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-7.5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 20.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0.05, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 20.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
+}
+
+TEST(shapeIntersection, planesphere)
+{
+  Sphere s(10);
+  Plane hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-5, 0, 0))));
+
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, halfspacebox)
+{
+  Box s(5, 10, 20);
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+  
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-1.25, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 3.75) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-0.625, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 3.75) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-0.625, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 1.25) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-1.875, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 1.25) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-1.875, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5.01) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0.005, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5.01) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0.005, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersection, planebox)
+{
+  Box s(5, 10, 20);
+  Plane hs(Vec3f(1, 0, 0), 0);
+  
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 1.25) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(1.25, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 1.25) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(1.25, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 1.25) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-1.25, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-1.25, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 1.25) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.51, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(transform.getQuatRotation()), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersection, halfspacecapsule)
+{
+  Capsule s(5, 10);
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-2.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-1.25, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-3.75, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-3.75, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0.05, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 1, 0), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -2.5, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -1.25, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -3.75, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -3.75, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0.05, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 0, 1), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 12.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -3.75)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 12.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -6.25)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -6.25))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 20.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0.05)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 20.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, planecapsule)
+{
+  Capsule s(5, 10);
+  Plane hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(2.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-2.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Plane(Vec3f(0, 1, 0), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 2.5, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -2.5, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Plane(Vec3f(0, 0, 1), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, 1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, halfspacecylinder)
+{
+  Cylinder s(5, 10);
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-2.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-1.25, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-1.25, 0, 0))));
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-3.75, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-3.75, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0.05, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0.05, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 1, 0), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -2.5, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -1.25, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -1.25, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -3.75, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -3.75, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0.05, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0.05, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 0, 1), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -1.25)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -1.25))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -3.75)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0.05)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, planecylinder)
+{
+  Cylinder s(5, 10);
+  Plane hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(2.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(2.5, 0, 0))));
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-2.5, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-2.5, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Plane(Vec3f(0, 1, 0), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 2.5, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 2.5, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -2.5, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -2.5, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Plane(Vec3f(0, 0, 1), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, 1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+
+TEST(shapeIntersection, halfspacecone)
+{
+  Cone s(5, 10);
+  Halfspace hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-2.5, 0, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-2.5, 0, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-1.25, 0, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-1.25, 0, -5))));
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-3.75, 0, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-3.75, 0, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0.05, 0, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0.05, 0, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 1, 0), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -2.5, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -2.5, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -1.25, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -1.25, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -3.75, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -3.75, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0.05, -5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0.05, -5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Halfspace(Vec3f(0, 0, 1), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -1.25)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 7.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -1.25))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -3.75)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -3.75))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0.05)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 5.1)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 10.1) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0.05))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -5.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersection, planecone)
+{
+  Cone s(5, 10);
+  Plane hs(Vec3f(1, 0, 0), 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL depth;
+  Vec3f normal;
+  bool res;
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)) || normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(2.5, 0, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(2.5, 0, -2.5))));
+  
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(-1, 0, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(-2.5, 0, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-2.5, 0, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(-1, 0, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(-2.5, 0, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(-5.1, 0, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Plane(Vec3f(0, 1, 0), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)) || normal.equal(Vec3f(0, 1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 2.5, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 2.5, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, -1, 0)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, -2.5, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -2.5, 0)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, -1, 0))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, -2.5, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, -5.1, 0)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+
+
+
+  hs = Plane(Vec3f(0, 0, 1), 0);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)) || normal.equal(Vec3f(0, 0, 1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 0)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform, &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))) || normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 0))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, 1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, 2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, 1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, 2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(Vec3f(0, 0, -1)));
+  ASSERT_TRUE(contact.equal(Vec3f(0, 0, -2.5)));
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -2.5)), &contact, &depth, &normal);
+  ASSERT_TRUE(res);
+  ASSERT_TRUE(std::abs(depth - 2.5) < 0.001);
+  ASSERT_TRUE(normal.equal(transform.getQuatRotation().transform(Vec3f(0, 0, -1))));
+  ASSERT_TRUE(contact.equal(transform.transform(Vec3f(0, 0, -2.5))));
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, Transform3f(), hs, Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeIntersect(s, transform, hs, transform * Transform3f(Vec3f(0, 0, -10.1)), &contact, &depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+
+
+TEST(shapeDistance, spheresphere)
+{  
+  Sphere s1(20);
+  Sphere s2(10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist = -1;
+  
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 10) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
+  ASSERT_TRUE(fabs(dist - 10) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  // this is one problem: the precise is low sometimes
+  ASSERT_TRUE(fabs(dist - 10) < 0.1);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.06);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
+  ASSERT_TRUE(fabs(dist - 10) < 0.1);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.1);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeDistance, boxbox)
+{                      
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 5) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 5) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeDistance, boxsphere)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+  
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.05);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 17.5) < 0.001);
+  ASSERT_TRUE(res);
+  
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 17.5) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeDistance, cylindercylinder)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+
+
+TEST(shapeDistance, conecone)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 1);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 1);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeDistance, conecylinder)
+{
+  Cylinder s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.1);
+  ASSERT_TRUE(res);
+}
+
+
+
+TEST(shapeIntersectionGJK, spheresphere)
+{
+  Sphere s1(20);
+  Sphere s2(10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  Vec3f contact;
+  FCL_REAL penetration_depth;
+  Vec3f normal;  
+  bool res;
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res); 
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(40, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(40, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(30, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(30, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(30.01, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.9, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-29.9, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-29.9, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(-30, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(-30, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(-30.01, 0, 0)), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersectionGJK, boxbox)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL penetration_depth;
+  Vec3f normal;  
+  bool res;
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(15, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(15.01, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+
+  Quaternion3f q;
+  q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(q), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+  
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(q), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersectionGJK, spherebox)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL penetration_depth;
+  Vec3f normal;  
+  bool res;
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.5, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.51, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(22.4, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersectionGJK, cylindercylinder)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL penetration_depth;
+  Vec3f normal;  
+  bool res;
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeIntersectionGJK, conecone)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL penetration_depth;
+  Vec3f normal;  
+  bool res;
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeIntersectionGJK, conecylinder)
+{
+  Cylinder s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  Vec3f contact;
+  FCL_REAL penetration_depth;
+  Vec3f normal;  
+  bool res;
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform, &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(9.9, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(10, 0, 0)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 9.9)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+  res = solver2.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10)), &contact, &penetration_depth, &normal);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), NULL, NULL, NULL);
+  ASSERT_FALSE(res);
+  res = solver2.shapeIntersect(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 10.1)), &contact, &penetration_depth, &normal);
+  ASSERT_FALSE(res);
+}
+
+
+TEST(shapeIntersectionGJK, spheretriangle)
+{
+  Sphere s(10);
+  Vec3f t[3];
+  t[0].setValue(20, 0, 0);
+  t[1].setValue(-20, 0, 0);
+  t[2].setValue(0, 20, 0);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+
+  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+
+  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+
+  t[0].setValue(30, 0, 0);
+  t[1].setValue(9.9, -20, 0);
+  t[2].setValue(9.9, 20, 0);
+  res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+
+  res =  solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL);
+  ASSERT_TRUE(res);
+}
+
+
+
+
+TEST(shapeDistanceGJK, spheresphere)
+{  
+  Sphere s1(20);
+  Sphere s2(10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist = -1;
+  
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 10) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist);
+  ASSERT_TRUE(fabs(dist - 10) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 10) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist);
+  ASSERT_TRUE(fabs(dist - 10) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(shapeDistanceGJK, boxbox)
+{                      
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 5) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 5) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeDistanceGJK, boxsphere)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.01);
+  ASSERT_TRUE(res);
+  
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.01);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 17.5) < 0.001);
+  ASSERT_TRUE(res);
+  
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 17.5) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeDistanceGJK, cylindercylinder)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+
+
+TEST(shapeDistanceGJK, conecone)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+TEST(shapeDistanceGJK, conecylinder)
+{
+  Cylinder s1(5, 10);
+  Cone s2(5, 10);
+
+  Transform3f transform;
+  generateRandomTransform(extents, transform);
+
+  bool res;
+  FCL_REAL dist;
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform, &dist);
+  ASSERT_TRUE(dist < 0);
+  ASSERT_FALSE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 0.1) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+
+  res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
+  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(res);
+}
+
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
diff --git a/trunk/fcl/test/test_fcl_utility.h b/trunk/fcl/test/test_fcl_utility.h
index 7d20645b6464b5e753dbe11ca095625380341f32..bdbfadfbd2f877b888f42fd3d18d9ccc4f5bf745 100644
--- a/trunk/fcl/test/test_fcl_utility.h
+++ b/trunk/fcl/test/test_fcl_utility.h
@@ -62,6 +62,15 @@ void generateRandomTransforms_ccd(FCL_REAL extents[6], std::vector<Transform3f>&
                                  const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
                                  const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2);
 
+
+/// @ brief Structure for minimum distance between two meshes and the corresponding nearest point pair
+struct DistanceRes
+{
+  double distance;
+  Vec3f p1;
+  Vec3f p2;
+};
+
 /// @brief Collision data stores the collision request and the result given by collision algorithm. 
 struct CollisionData
 {