diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 19f3955a78eaee2fe6a0f49d73031e4609edb7f6..30bf764a273f6b203f0797bd5301bd5c968a7666 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -102,3 +102,25 @@ 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)
+
+add_executable(test_fcl_broadphase test/test_fcl_broadphase.cpp test/test_fcl_utility.cpp)
+target_link_libraries(test_fcl_broadphase ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
+GTEST_ADD_TESTS(test_fcl_broadphase "" test/test_fcl_broadphase.cpp)
+
+add_executable(test_fcl_shape_mesh_consistency test/test_fcl_shape_mesh_consistency.cpp test/test_fcl_utility.cpp)
+target_link_libraries(test_fcl_shape_mesh_consistency ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
+GTEST_ADD_TESTS(test_fcl_shape_mesh_consistency "" test/test_fcl_shape_mesh_consistency.cpp)
+
+add_executable(test_fcl_frontlist test/test_fcl_frontlist.cpp test/test_fcl_utility.cpp)
+target_link_libraries(test_fcl_frontlist ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
+GTEST_ADD_TESTS(test_fcl_frontlist "" test/test_fcl_frontlist.cpp)
+
+add_executable(test_fcl_math test/test_fcl_math.cpp test/test_fcl_utility.cpp)
+target_link_libraries(test_fcl_math ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
+GTEST_ADD_TESTS(test_fcl_math "" test/test_fcl_math.cpp)
+
+if (FCL_HAVE_OCTOMAP)
+  add_executable(test_fcl_octomap test/test_fcl_octomap.cpp test/test_fcl_utility.cpp)
+  target_link_libraries(test_fcl_octomap ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
+  GTEST_ADD_TESTS(test_fcl_octomap "" test/test_fcl_octomap.cpp)
+endif()
diff --git a/trunk/fcl/src/narrowphase/narrowphase.cpp b/trunk/fcl/src/narrowphase/narrowphase.cpp
index 18afc5913c9400437b93fda7ee364e88b6afa192..a380308db5caf09d270cc549db2327b5a836f8ab 100644
--- a/trunk/fcl/src/narrowphase/narrowphase.cpp
+++ b/trunk/fcl/src/narrowphase/narrowphase.cpp
@@ -1876,7 +1876,7 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
     // the contact point is the intersect of axis with the plane
     // the normal is the direction to avoid intersection
     // the depth is the minimum distance to resolve the collision
-    if(d1 * d2 < 0)
+    if(d1 * d2 < -planeIntersectTolerance<FCL_REAL>())
     {
       if(abs_d1 < abs_d2)
       {
diff --git a/trunk/fcl/test/test_fcl_broadphase.cpp b/trunk/fcl/test/test_fcl_broadphase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d4d0155b74836389c5998fde02200f2dc7a7b251
--- /dev/null
+++ b/trunk/fcl/test/test_fcl_broadphase.cpp
@@ -0,0 +1,1121 @@
+/*
+ * 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/broadphase/broadphase.h"
+#include "fcl/shape/geometric_shape_to_BVH_model.h"
+#include "fcl/math/transform.h"
+#include "test_fcl_utility.h"
+#include <gtest/gtest.h>
+
+#if USE_GOOGLEHASH
+#include <sparsehash/sparse_hash_map>
+#include <sparsehash/dense_hash_map>
+#include <hash_map>
+#endif
+
+#include <boost/math/constants/constants.hpp>
+
+using namespace fcl;
+
+struct TStruct
+{
+  std::vector<double> records;
+  double overall_time;
+
+  TStruct() { overall_time = 0; }
+  
+  void push_back(double t)
+  {
+    records.push_back(t);
+    overall_time += t;
+  }
+};
+
+/// @brief Generate environment with 3 * n objects: n boxes, n spheres and n cylinders.
+void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
+
+/// @brief Generate environment with 3 * n objects for self distance, so we try to make sure none of them collide with each other.
+void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
+
+/// @brief Generate environment with 3 * n objects, but all in meshes.
+void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
+
+/// @brief Generate environment with 3 * n objects for self distance, but all in meshes.
+void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
+
+/// @brief test for broad phase collision and self collision
+void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false);
+
+/// @brief test for broad phase distance
+void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh = false);
+
+/// @brief test for broad phase self distance
+void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh = false);
+
+/// @brief test for broad phase update
+void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false);
+
+FCL_REAL DELTA = 0.01;
+
+
+#if USE_GOOGLEHASH
+template<typename U, typename V>
+struct GoogleSparseHashTable : public google::sparse_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> > {};
+
+template<typename U, typename V>
+struct GoogleDenseHashTable : public google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >
+{
+  GoogleDenseHashTable() : google::dense_hash_map<U, V, std::tr1::hash<size_t>, std::equal_to<size_t> >()
+  {
+    this->set_empty_key(NULL);
+  }
+};
+#endif
+
+/// check the update, only return collision or not
+TEST(test_core_bf, broad_phase_update_collision_binary)
+{
+  broad_phase_update_collision_test(2000, 100, 1000, 1, false);
+  broad_phase_update_collision_test(2000, 1000, 1000, 1, false);
+}
+
+/// check the update, return 10 contacts
+TEST(test_core_bf, broad_phase_update_collision)
+{
+  broad_phase_update_collision_test(2000, 100, 1000, 10, false);
+  broad_phase_update_collision_test(2000, 1000, 1000, 10, false);
+}
+
+/// check the update, exhaustive
+TEST(test_core_bf, broad_phase_update_collision_exhaustive)
+{
+  broad_phase_update_collision_test(2000, 100, 1000, 1, true);
+  broad_phase_update_collision_test(2000, 1000, 1000, 1, true);
+}
+
+/// check broad phase distance
+TEST(test_core_bf, broad_phase_distance)
+{
+  broad_phase_distance_test(200, 100, 100);
+  broad_phase_distance_test(200, 1000, 100);
+  broad_phase_distance_test(2000, 100, 100);
+  broad_phase_distance_test(2000, 1000, 100);
+}
+
+/// check broad phase self distance
+TEST(test_core_bf, broad_phase_self_distance)
+{
+  broad_phase_self_distance_test(200, 512);
+  broad_phase_self_distance_test(200, 1000);
+  broad_phase_self_distance_test(200, 5000);
+}
+
+/// check broad phase collision and self collision, only return collision or not
+TEST(test_core_bf, broad_phase_collision_binary)
+{
+  broad_phase_collision_test(2000, 100, 1000, 1, false);
+  broad_phase_collision_test(2000, 1000, 1000, 1, false);
+  broad_phase_collision_test(2000, 100, 1000, 1, true);
+  broad_phase_collision_test(2000, 1000, 1000, 1, true);
+}
+
+/// check broad phase collision and self collision, return 10 contacts
+TEST(test_core_bf, broad_phase_collision)
+{
+  broad_phase_collision_test(2000, 100, 1000, 10, false);
+  broad_phase_collision_test(2000, 1000, 1000, 10, false);
+}
+
+/// check broad phase update, in mesh, only return collision or not
+TEST(test_core_mesh_bf, broad_phase_update_collision_mesh_binary)
+{
+  broad_phase_update_collision_test(2000, 100, 1000, false, true);
+  broad_phase_update_collision_test(2000, 1000, 1000, false, true);
+}
+
+/// check broad phase update, in mesh, return 10 contacts
+TEST(test_core_mesh_bf, broad_phase_update_collision_mesh)
+{
+  broad_phase_update_collision_test(2000, 100, 1000, 10, false, true);
+  broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true);
+}
+
+/// check broad phase update, in mesh, exhaustive
+TEST(test_core_mesh_bf, broad_phase_update_collision_mesh_exhaustive)
+{
+  broad_phase_update_collision_test(2000, 100, 1000, 1, true, true);
+  broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true);
+}
+
+/// check broad phase distance
+TEST(test_core_mesh_bf, broad_phase_distance_mesh)
+{
+  broad_phase_distance_test(200, 100, 100, true);
+  broad_phase_distance_test(200, 1000, 100, true);
+  broad_phase_distance_test(2000, 100, 100, true);
+  broad_phase_distance_test(2000, 1000, 100, true);
+}
+
+/// check broad phase self distance
+TEST(test_core_mesh_bf, broad_phase_self_distance_mesh)
+{
+  broad_phase_self_distance_test(200, 512, true);
+  broad_phase_self_distance_test(200, 1000, true);
+  broad_phase_self_distance_test(200, 5000, true);
+}
+
+/// check broad phase collision and self collision, return only collision or not, in mesh
+TEST(test_core_mesh_bf, broad_phase_collision_mesh_binary)
+{
+  broad_phase_collision_test(2000, 100, 1000, 1, false, true);
+  broad_phase_collision_test(2000, 1000, 1000, 1, false, true);
+}
+
+/// check broad phase collision and self collision, return 10 contacts, in mesh
+TEST(test_core_mesh_bf, broad_phase_collision_mesh)
+{
+  broad_phase_collision_test(2000, 100, 1000, 10, false, true);
+  broad_phase_collision_test(2000, 1000, 1000, 10, false, true);
+}
+
+/// check broad phase collision and self collision, exhaustive, in mesh
+TEST(test_core_mesh_bf, broad_phase_collision_mesh_exhaustive)
+{
+  broad_phase_collision_test(2000, 100, 1000, 1, true, true);
+  broad_phase_collision_test(2000, 1000, 1000, 1, true, true);
+}
+
+
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+
+void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
+{
+  FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
+  std::vector<Transform3f> transforms;
+
+  generateRandomTransforms(extents, transforms, n);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Box* box = new Box(5, 10, 20);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Sphere* sphere = new Sphere(30);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(sphere), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Cylinder* cylinder = new Cylinder(10, 40);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(cylinder), transforms[i]));
+  }
+}
+
+void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
+{
+  FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
+  std::vector<Transform3f> transforms;
+
+  generateRandomTransforms(extents, transforms, n);
+  Box box(5, 10, 20);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, box, Transform3f());
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  Sphere sphere(30);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, sphere, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  Cylinder cylinder(10, 40);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
+  }
+}
+
+void generateSelfDistanceEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
+{
+  int n_edge = std::floor(std::pow(n, 1/3.0));
+
+  FCL_REAL step_size = env_scale * 2 / n_edge;
+  FCL_REAL delta_size = step_size * 0.05;
+  FCL_REAL single_size = step_size - 2 * delta_size;
+  
+  std::size_t i = 0;
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Box* box = new Box(single_size, single_size, single_size);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(box),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Sphere* sphere = new Sphere(single_size / 2);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(sphere),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Cylinder* cylinder = new Cylinder(single_size / 2, single_size);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(cylinder),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Cone* cone = new Cone(single_size / 2, single_size);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(cone),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+}
+
+void generateSelfDistanceEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
+{
+  int n_edge = std::floor(std::pow(n, 1/3.0));
+
+  FCL_REAL step_size = env_scale * 2 / n_edge;
+  FCL_REAL delta_size = step_size * 0.05;
+  FCL_REAL single_size = step_size - 2 * delta_size;
+  
+  std::size_t i = 0;
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Box box(single_size, single_size, single_size);
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, box, Transform3f());
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Sphere sphere(single_size / 2);
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, sphere, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Cylinder cylinder(single_size / 2, single_size);
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+
+  for(; i < n_edge * n_edge * n_edge / 4; ++i)
+  {
+    int x = i % (n_edge * n_edge);
+    int y = (i - n_edge * n_edge * x) % n_edge;
+    int z = i - n_edge * n_edge * x - n_edge * y;
+
+    Cone cone(single_size / 2, single_size);
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, cone, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model),
+                                      Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, 
+                                                            y * step_size + delta_size + 0.5 * single_size - env_scale,
+                                                            z * step_size + delta_size + 0.5 * single_size - env_scale))));
+  }
+}
+
+
+void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh)
+{
+  std::vector<TStruct> ts;
+  std::vector<Timer> timers;
+
+  std::vector<CollisionObject*> env;
+  if(use_mesh)
+    generateEnvironmentsMesh(env, env_scale, env_size);
+  else
+    generateEnvironments(env, env_scale, env_size);
+
+  std::vector<CollisionObject*> query;
+  if(use_mesh)
+    generateEnvironmentsMesh(query, env_scale, query_size);
+  else
+    generateEnvironments(query, env_scale, query_size);
+
+  std::vector<BroadPhaseCollisionManager*> managers;
+  
+  managers.push_back(new NaiveCollisionManager());
+  managers.push_back(new SSaPCollisionManager());
+  managers.push_back(new SaPCollisionManager());
+  managers.push_back(new IntervalTreeCollisionManager());
+  
+  Vec3f lower_limit, upper_limit;
+  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
+  // FCL_REAL ncell_per_axis = std::pow((FCL_REAL)env_size / 10, 1 / 3.0);
+  FCL_REAL ncell_per_axis = 20;
+  FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis);
+  // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit));
+#if USE_GOOGLEHASH
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
+#endif
+  managers.push_back(new DynamicAABBTreeCollisionManager());
+
+  managers.push_back(new DynamicAABBTreeCollisionManager_Array());
+
+  {
+    DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  {
+    DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  ts.resize(managers.size());
+  timers.resize(managers.size());
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->registerObjects(env);
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->setup();
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+  std::vector<CollisionData> self_data(managers.size());
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    if(exhaustive) self_data[i].request.num_max_contacts = 100000;
+    else self_data[i].request.num_max_contacts = num_max_contacts;
+  }
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->collide(&self_data[i], defaultCollisionFunction);
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+  for(size_t i = 0; i < managers.size(); ++i)
+    std::cout << self_data[i].result.numContacts() << " ";
+  std::cout << std::endl;
+
+  if(exhaustive)
+  {
+    for(size_t i = 1; i < managers.size(); ++i)
+      ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
+  }
+  else
+  {
+    std::vector<bool> self_res(managers.size());
+    for(size_t i = 0; i < self_res.size(); ++i)
+      self_res[i] = (self_data[i].result.numContacts() > 0);
+  
+    for(size_t i = 1; i < self_res.size(); ++i)
+      ASSERT_TRUE(self_res[0] == self_res[i]);
+
+    for(size_t i = 1; i < managers.size(); ++i)
+      ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
+  }
+
+
+  for(size_t i = 0; i < query.size(); ++i)
+  {
+    std::vector<CollisionData> query_data(managers.size());
+    for(size_t j = 0; j < query_data.size(); ++j)
+    {
+      if(exhaustive) query_data[j].request.num_max_contacts = 100000;
+      else query_data[j].request.num_max_contacts = num_max_contacts;
+    }
+
+    for(size_t j = 0; j < query_data.size(); ++j)
+    {
+      timers[j].start();
+      managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction);
+      timers[j].stop();
+      ts[j].push_back(timers[j].getElapsedTime());
+    }
+
+
+    // for(size_t j = 0; j < managers.size(); ++j)
+    //   std::cout << query_data[j].result.numContacts() << " ";
+    // std::cout << std::endl;
+    
+    if(exhaustive)
+    {
+      for(size_t j = 1; j < managers.size(); ++j)
+        ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
+    }
+    else
+    {
+      std::vector<bool> query_res(managers.size());
+      for(size_t j = 0; j < query_res.size(); ++j)
+        query_res[j] = (query_data[j].result.numContacts() > 0);
+      for(size_t j = 1; j < query_res.size(); ++j)
+        ASSERT_TRUE(query_res[0] == query_res[j]);
+
+      for(size_t j = 1; j < managers.size(); ++j)
+        ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
+    }
+  }
+
+  for(size_t i = 0; i < env.size(); ++i)
+    delete env[i];
+  for(size_t i = 0; i < query.size(); ++i)
+    delete query[i];
+
+  for(size_t i = 0; i < managers.size(); ++i)
+    delete managers[i];     
+
+  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
+  size_t w = 7;
+
+  std::cout << "collision timing summary" << std::endl;
+  std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
+  std::cout << "register time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[0] << " ";
+  std::cout << std::endl;
+
+  std::cout << "setup time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[1] << " ";
+  std::cout << std::endl;
+
+  std::cout << "self collision time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[2] << " ";
+  std::cout << std::endl;
+
+  std::cout << "collision time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+  {
+    FCL_REAL tmp = 0;
+    for(size_t j = 3; j < ts[i].records.size(); ++j)
+      tmp += ts[i].records[j];
+    std::cout << std::setw(w) << tmp << " ";
+  }
+  std::cout << std::endl;
+
+
+  std::cout << "overall time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].overall_time << " ";
+  std::cout << std::endl;
+  std::cout << std::endl;
+
+}
+
+void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh)
+{
+  std::vector<TStruct> ts;
+  std::vector<Timer> timers;
+
+  std::vector<CollisionObject*> env;
+  if(use_mesh)
+    generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size);
+  else
+    generateSelfDistanceEnvironments(env, env_scale, env_size);
+  
+  std::vector<BroadPhaseCollisionManager*> managers;
+  
+  managers.push_back(new NaiveCollisionManager());
+  managers.push_back(new SSaPCollisionManager());
+  managers.push_back(new SaPCollisionManager());
+  managers.push_back(new IntervalTreeCollisionManager());
+  
+  Vec3f lower_limit, upper_limit;
+  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
+  FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, (upper_limit[1] - lower_limit[1]) / 5), (upper_limit[2] - lower_limit[2]) / 5);
+  // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit));
+#if USE_GOOGLEHASH
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
+#endif
+  managers.push_back(new DynamicAABBTreeCollisionManager());
+  managers.push_back(new DynamicAABBTreeCollisionManager_Array());
+
+  {
+    DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  {
+    DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  ts.resize(managers.size());
+  timers.resize(managers.size());
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->registerObjects(env);
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->setup();
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+ 
+
+  std::vector<DistanceData> self_data(managers.size());
+                                      
+  for(size_t i = 0; i < self_data.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->distance(&self_data[i], defaultDistanceFunction);
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+    // std::cout << self_data[i].result.min_distance << " ";
+  }
+  // std::cout << std::endl;
+
+  for(size_t i = 1; i < managers.size(); ++i)
+    ASSERT_TRUE(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < DELTA ||
+                fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) / fabs(self_data[0].result.min_distance) < DELTA);
+
+  for(size_t i = 0; i < env.size(); ++i)
+    delete env[i];
+
+  for(size_t i = 0; i < managers.size(); ++i)
+    delete managers[i];     
+
+  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
+  size_t w = 7;
+
+  std::cout << "self distance timing summary" << std::endl;
+  std::cout << env.size() << " objs" << std::endl;
+  std::cout << "register time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[0] << " ";
+  std::cout << std::endl;
+
+  std::cout << "setup time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[1] << " ";
+  std::cout << std::endl;
+
+  std::cout << "self distance time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[2] << " ";
+  std::cout << std::endl;
+
+  std::cout << "overall time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].overall_time << " ";
+  std::cout << std::endl;
+  std::cout << std::endl;
+}
+
+
+void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh)
+{
+  std::vector<TStruct> ts;
+  std::vector<Timer> timers;
+
+  std::vector<CollisionObject*> env;
+  if(use_mesh)
+    generateEnvironmentsMesh(env, env_scale, env_size);
+  else
+    generateEnvironments(env, env_scale, env_size);
+
+  std::vector<CollisionObject*> query;
+
+  BroadPhaseCollisionManager* manager = new NaiveCollisionManager();
+  for(std::size_t i = 0; i < env.size(); ++i)
+    manager->registerObject(env[i]);
+  manager->setup();
+
+  while(1)
+  {
+    std::vector<CollisionObject*> candidates;
+    if(use_mesh)
+      generateEnvironmentsMesh(candidates, env_scale, query_size);
+    else
+      generateEnvironments(candidates, env_scale, query_size);
+
+    for(std::size_t i = 0; i < candidates.size(); ++i)
+    {
+      CollisionData query_data;
+      manager->collide(candidates[i], &query_data, defaultCollisionFunction);
+      if(query_data.result.numContacts() == 0)
+        query.push_back(candidates[i]);
+      else
+        delete candidates[i];
+      if(query.size() == query_size) break;
+    }
+
+    if(query.size() == query_size) break;
+  }
+
+  delete manager;
+
+  std::vector<BroadPhaseCollisionManager*> managers;
+
+  managers.push_back(new NaiveCollisionManager());
+  managers.push_back(new SSaPCollisionManager());
+  managers.push_back(new SaPCollisionManager());
+  managers.push_back(new IntervalTreeCollisionManager());
+  
+  Vec3f lower_limit, upper_limit;
+  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
+  FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20);
+  // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit));
+#if USE_GOOGLEHASH
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
+#endif
+  managers.push_back(new DynamicAABBTreeCollisionManager());
+  managers.push_back(new DynamicAABBTreeCollisionManager_Array());
+
+  {
+    DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  {
+    DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  ts.resize(managers.size());
+  timers.resize(managers.size());
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->registerObjects(env);
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->setup();
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+
+  for(size_t i = 0; i < query.size(); ++i)
+  {
+    std::vector<DistanceData> query_data(managers.size());
+    for(size_t j = 0; j < managers.size(); ++j)
+    {
+      timers[j].start();
+      managers[j]->distance(query[i], &query_data[j], defaultDistanceFunction);
+      timers[j].stop();
+      ts[j].push_back(timers[j].getElapsedTime());
+      // std::cout << query_data[j].result.min_distance << " ";
+    }
+    // std::cout << std::endl;
+
+    for(size_t j = 1; j < managers.size(); ++j)
+      ASSERT_TRUE(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < DELTA ||
+                  fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) / fabs(query_data[0].result.min_distance) < DELTA);
+  }
+
+
+  for(std::size_t i = 0; i < env.size(); ++i)
+    delete env[i];
+  for(std::size_t i = 0; i < query.size(); ++i)
+    delete query[i];
+
+  for(size_t i = 0; i < managers.size(); ++i)
+    delete managers[i];
+
+
+  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
+  size_t w = 7;
+
+  std::cout << "distance timing summary" << std::endl;
+  std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
+  std::cout << "register time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[0] << " ";
+  std::cout << std::endl;
+
+  std::cout << "setup time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[1] << " ";
+  std::cout << std::endl;
+
+  std::cout << "distance time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+  {
+    FCL_REAL tmp = 0;
+    for(size_t j = 2; j < ts[i].records.size(); ++j)
+      tmp += ts[i].records[j];
+    std::cout << std::setw(w) << tmp << " ";
+  }
+  std::cout << std::endl;
+
+  std::cout << "overall time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].overall_time << " ";
+  std::cout << std::endl;
+  std::cout << std::endl;
+}
+
+
+void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh)
+{
+  std::vector<TStruct> ts;
+  std::vector<Timer> timers;
+
+  std::vector<CollisionObject*> env;
+  if(use_mesh)
+    generateEnvironmentsMesh(env, env_scale, env_size);
+  else
+    generateEnvironments(env, env_scale, env_size);
+
+  std::vector<CollisionObject*> query;
+  if(use_mesh)
+    generateEnvironmentsMesh(query, env_scale, query_size); 
+  else
+    generateEnvironments(query, env_scale, query_size); 
+
+  std::vector<BroadPhaseCollisionManager*> managers;
+  
+  managers.push_back(new NaiveCollisionManager());
+  managers.push_back(new SSaPCollisionManager());
+
+  
+  managers.push_back(new SaPCollisionManager());
+  managers.push_back(new IntervalTreeCollisionManager());
+  
+  Vec3f lower_limit, upper_limit;
+  SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit);
+  FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20);
+  // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash> >(cell_size, lower_limit, upper_limit));
+#if USE_GOOGLEHASH
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleSparseHashTable> >(cell_size, lower_limit, upper_limit));
+  managers.push_back(new SpatialHashingCollisionManager<SparseHashTable<AABB, CollisionObject*, SpatialHash, GoogleDenseHashTable> >(cell_size, lower_limit, upper_limit));
+#endif
+  managers.push_back(new DynamicAABBTreeCollisionManager());
+  managers.push_back(new DynamicAABBTreeCollisionManager_Array());
+
+  {
+    DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  {
+    DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array();
+    m->tree_init_level = 2;
+    managers.push_back(m);
+  }
+
+  ts.resize(managers.size());
+  timers.resize(managers.size());
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->registerObjects(env);
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+  
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->setup();
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+  // update the environment
+  FCL_REAL delta_angle_max = 10 / 360.0 * 2 * boost::math::constants::pi<FCL_REAL>();
+  FCL_REAL delta_trans_max = 0.01 * env_scale;
+  for(size_t i = 0; i < env.size(); ++i)
+  {
+    FCL_REAL rand_angle_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
+    FCL_REAL rand_trans_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
+    FCL_REAL rand_angle_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
+    FCL_REAL rand_trans_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
+    FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max;
+    FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max;
+
+    Quaternion3f q1, q2, q3;
+    q1.fromAxisAngle(Vec3f(1, 0, 0), rand_angle_x);
+    q2.fromAxisAngle(Vec3f(0, 1, 0), rand_angle_y);
+    q3.fromAxisAngle(Vec3f(0, 0, 1), rand_angle_z);
+    Quaternion3f q = q1 * q2 * q3;
+    Matrix3f dR;
+    q.toRotation(dR);
+    Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z);
+    
+    Matrix3f R = env[i]->getRotation();
+    Vec3f T = env[i]->getTranslation();
+    env[i]->setTransform(dR * R, dR * T + dT);
+    env[i]->computeAABB();
+  }
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->update();
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+  std::vector<CollisionData> self_data(managers.size());
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    if(exhaustive) self_data[i].request.num_max_contacts = 100000;
+    else self_data[i].request.num_max_contacts = num_max_contacts;
+  }
+
+  for(size_t i = 0; i < managers.size(); ++i)
+  {
+    timers[i].start();
+    managers[i]->collide(&self_data[i], defaultCollisionFunction);
+    timers[i].stop();
+    ts[i].push_back(timers[i].getElapsedTime());
+  }
+
+
+  for(size_t i = 0; i < managers.size(); ++i)
+    std::cout << self_data[i].result.numContacts() << " ";
+  std::cout << std::endl;
+
+  if(exhaustive)
+  {
+    for(size_t i = 1; i < managers.size(); ++i)
+      ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
+  }
+  else
+  {
+    std::vector<bool> self_res(managers.size());
+    for(size_t i = 0; i < self_res.size(); ++i)
+      self_res[i] = (self_data[i].result.numContacts() > 0);
+  
+    for(size_t i = 1; i < self_res.size(); ++i)
+      ASSERT_TRUE(self_res[0] == self_res[i]);
+
+    for(size_t i = 1; i < managers.size(); ++i)
+      ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
+  }
+
+
+  for(size_t i = 0; i < query.size(); ++i)
+  {
+    std::vector<CollisionData> query_data(managers.size());
+    for(size_t j = 0; j < query_data.size(); ++j)
+    {
+      if(exhaustive) query_data[j].request.num_max_contacts = 100000;
+      else query_data[j].request.num_max_contacts = num_max_contacts;
+    }
+
+    for(size_t j = 0; j < query_data.size(); ++j)
+    {
+      timers[j].start();
+      managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction);
+      timers[j].stop();
+      ts[j].push_back(timers[j].getElapsedTime());
+    }
+
+
+    // for(size_t j = 0; j < managers.size(); ++j)
+    //   std::cout << query_data[j].result.numContacts() << " ";
+    // std::cout << std::endl;
+    
+    if(exhaustive)
+    {
+      for(size_t j = 1; j < managers.size(); ++j)
+        ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
+    }
+    else
+    {
+      std::vector<bool> query_res(managers.size());
+      for(size_t j = 0; j < query_res.size(); ++j)
+        query_res[j] = (query_data[j].result.numContacts() > 0);
+      for(size_t j = 1; j < query_res.size(); ++j)
+        ASSERT_TRUE(query_res[0] == query_res[j]);
+
+      for(size_t j = 1; j < managers.size(); ++j)
+        ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
+    }
+  }
+
+
+  for(size_t i = 0; i < env.size(); ++i)
+    delete env[i];
+  for(size_t i = 0; i < query.size(); ++i)
+    delete query[i];
+
+  for(size_t i = 0; i < managers.size(); ++i)
+    delete managers[i];
+  
+
+  std::cout.setf(std::ios_base::left, std::ios_base::adjustfield);
+  size_t w = 7;
+
+  std::cout << "collision timing summary" << std::endl;
+  std::cout << env_size << " objs, " << query_size << " queries" << std::endl;
+  std::cout << "register time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[0] << " ";
+  std::cout << std::endl;
+
+  std::cout << "setup time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[1] << " ";
+  std::cout << std::endl;
+
+  std::cout << "update time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[2] << " ";
+  std::cout << std::endl;
+
+  std::cout << "self collision time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].records[3] << " ";
+  std::cout << std::endl;
+
+  std::cout << "collision time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+  {
+    FCL_REAL tmp = 0;
+    for(size_t j = 4; j < ts[i].records.size(); ++j)
+      tmp += ts[i].records[j];
+    std::cout << std::setw(w) << tmp << " ";
+  }
+  std::cout << std::endl;
+
+
+  std::cout << "overall time" << std::endl;
+  for(size_t i = 0; i < ts.size(); ++i)
+    std::cout << std::setw(w) << ts[i].overall_time << " ";
+  std::cout << std::endl;
+  std::cout << std::endl;
+}
+
+
+
diff --git a/trunk/fcl/test/test_fcl_frontlist.cpp b/trunk/fcl/test/test_fcl_frontlist.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aaf7dccd499762b83b6c1722d967ce1f22809174
--- /dev/null
+++ b/trunk/fcl/test/test_fcl_frontlist.cpp
@@ -0,0 +1,357 @@
+/*
+ * 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>
+
+using namespace fcl;
+
+template<typename BV>
+bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
+                             const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                             const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+                             SplitMethodType split_method,
+                             bool refit_bottomup, bool verbose);
+
+template<typename BV, typename TraversalNode>
+bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2,
+                                      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>
+bool collide_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, bool verbose);
+
+// TODO: randomly still have some runtime error
+TEST(collision_test, front_list)
+{
+  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
+  std::vector<Transform3f> transforms2; // t1
+  FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+  FCL_REAL delta_trans[] = {1, 1, 1};
+  std::size_t n = 10;
+  bool verbose = false;
+
+  generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n);
+
+  bool res, res2;
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<AABB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<KDOP<16> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<KDOP<18> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<KDOP<24> >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test_Oriented<RSS, MeshCollisionTraversalNodeRSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+  for(std::size_t i = 0; i < transforms.size(); ++i)
+  {
+    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+    ASSERT_TRUE(res == res2);
+    res = collide_Test<OBB>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    res2 = collide_front_list_Test_Oriented<OBB, MeshCollisionTraversalNodeOBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose);
+    ASSERT_TRUE(res == res2);
+  }
+
+}
+
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+
+template<typename BV>
+bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
+                             const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+                             const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+                             SplitMethodType split_method,
+                             bool refit_bottomup, 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));
+
+  BVHFrontList front_list;
+
+
+  std::vector<Vec3f> vertices1_new(vertices1.size());
+  for(std::size_t i = 0; i < vertices1_new.size(); ++i)
+  {
+    vertices1_new[i] = tf1.transform(vertices1[i]);
+  }
+
+  m1.beginModel();
+  m1.addSubModel(vertices1_new, triangles1);
+  m1.endModel();
+
+  m2.beginModel();
+  m2.addSubModel(vertices2, triangles2);
+  m2.endModel();
+
+  Transform3f pose1, pose2;
+
+  CollisionResult local_result;	
+  MeshCollisionTraversalNode<BV> node;
+
+  if(!initialize<BV>(node, m1, pose1, m2, pose2,
+                     CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
+    std::cout << "initialize error" << std::endl;
+
+  node.enable_statistics = verbose;
+
+  collide(&node, &front_list);
+
+  if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
+
+
+  // update the mesh
+  for(std::size_t i = 0; i < vertices1.size(); ++i)
+  {
+    vertices1_new[i] = tf2.transform(vertices1[i]);
+  }
+
+  m1.beginReplaceModel();
+  m1.replaceSubModel(vertices1_new);
+  m1.endReplaceModel(true, refit_bottomup);
+
+  m2.beginReplaceModel();
+  m2.replaceSubModel(vertices2);
+  m2.endReplaceModel(true, refit_bottomup);
+
+  local_result.clear();
+  collide(&node, &front_list);
+
+  if(local_result.numContacts() > 0)
+    return true;
+  else
+    return false;
+}
+
+
+
+
+template<typename BV, typename TraversalNode>
+bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2,
+                                      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<BV> m1;
+  BVHModel<BV> m2;
+  m1.bv_splitter.reset(new BVSplitter<BV>(split_method));
+  m2.bv_splitter.reset(new BVSplitter<BV>(split_method));
+
+  BVHFrontList front_list;
+
+  m1.beginModel();
+  m1.addSubModel(vertices1, triangles1);
+  m1.endModel();
+
+  m2.beginModel();
+  m2.addSubModel(vertices2, triangles2);
+  m2.endModel();
+
+  Transform3f pose1(tf1), pose2;
+
+  CollisionResult local_result;	
+  TraversalNode node;
+
+  if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2,
+                 CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
+    std::cout << "initialize error" << std::endl;
+
+  node.enable_statistics = verbose;
+
+  collide(&node, &front_list);
+
+  if(verbose) std::cout << "front list size " << front_list.size() << std::endl;
+
+
+  // update the mesh
+  pose1 = tf2;
+  if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2, CollisionRequest(), local_result))
+    std::cout << "initialize error" << std::endl;
+
+  local_result.clear();
+  collide(&node, &front_list);
+
+  if(local_result.numContacts() > 0)
+    return true;
+  else
+    return false;
+}
+
+
+template<typename BV>
+bool collide_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, 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;
+
+  CollisionResult local_result;	
+  MeshCollisionTraversalNode<BV> node;
+
+  if(!initialize<BV>(node, m1, pose1, m2, pose2,
+                     CollisionRequest(std::numeric_limits<int>::max(), false), 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
index cd3559aa844c7b67d0e8e93832b79c7fb920eb2d..ee198da8b6b1837e678df2787dcdd69fdd08413f 100644
--- a/trunk/fcl/test/test_fcl_geometric_shapes.cpp
+++ b/trunk/fcl/test/test_fcl_geometric_shapes.cpp
@@ -390,16 +390,16 @@ TEST(shapeIntersection, conecylinder)
   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);
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(10.01, 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);
+  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, 0, 0)), &solver1, request, result) > 0);
+  res = (collide(&s1, transform, &s2, transform * Transform3f(Vec3f(10.01, 0, 0)), &solver1, request, result) > 0);
   ASSERT_FALSE(res);
 
   res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 9.9)), NULL, NULL, NULL);
@@ -414,7 +414,7 @@ TEST(shapeIntersection, conecylinder)
   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);
+  res = solver1.shapeIntersect(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 10.01)), NULL, NULL, NULL);
   ASSERT_FALSE(res);
   result.clear();
   res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(0, 0, 10)), &solver1, request, result) > 0);
@@ -2018,15 +2018,15 @@ TEST(shapeDistance, conecylinder)
   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(fabs(dist - 0.1) < 0.01);
   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(fabs(dist - 0.1) < 0.02);
   ASSERT_TRUE(res);
 
   res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist);
-  ASSERT_TRUE(fabs(dist - 30) < 0.001);
+  ASSERT_TRUE(fabs(dist - 30) < 0.01);
   ASSERT_TRUE(res);
 
   res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist);
diff --git a/trunk/fcl/test/test_fcl_math.cpp b/trunk/fcl/test/test_fcl_math.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..755cacc1a370596bbc85538297a9c6a6afe7c24e
--- /dev/null
+++ b/trunk/fcl/test/test_fcl_math.cpp
@@ -0,0 +1,640 @@
+/*
+ * 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.
+ */
+
+
+#include "fcl/simd/math_simd_details.h"
+#include "fcl/math/vec_3f.h"
+#include "fcl/math/matrix_3f.h"
+#include "fcl/broadphase/morton.h"
+#include <gtest/gtest.h>
+
+using namespace fcl;
+
+TEST(vec_test, basic_vec32)
+{
+  typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
+  Vec3f32 v1(1.0f, 2.0f, 3.0f);
+  ASSERT_TRUE(v1[0] == 1.0f);
+  ASSERT_TRUE(v1[1] == 2.0f);
+  ASSERT_TRUE(v1[2] == 3.0f);
+
+  Vec3f32 v2 = v1;
+  Vec3f32 v3(3.3f, 4.3f, 5.3f);
+  v1 += v3;
+  ASSERT_TRUE(v1.equal(v2 + v3));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2 - v3));
+  v1 += v3;
+
+  v1 *= v3;
+  ASSERT_TRUE(v1.equal(v2 * v3));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2 / v3));
+  v1 *= v3;
+
+  v1 *= 2.0f;
+  ASSERT_TRUE(v1.equal(v2 * 2.0f));
+  v1 /= 2.0f;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= 2.0f;
+  ASSERT_TRUE(v1.equal(v2 / 2.0f));
+  v1 *= 2.0f;
+
+  v1 += 2.0f;
+  ASSERT_TRUE(v1.equal(v2 + 2.0f));
+  v1 -= 2.0f;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= 2.0f;
+  ASSERT_TRUE(v1.equal(v2 - 2.0f));
+  v1 += 2.0f;
+  
+  ASSERT_TRUE((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f)));
+
+  v1 = Vec3f32(1.0f, 2.0f, 3.0f);
+  v2 = Vec3f32(3.0f, 4.0f, 5.0f);
+  ASSERT_TRUE((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f)));
+  ASSERT_TRUE(abs(v1.dot(v2) - 26) < 1e-5);
+
+  v1 = Vec3f32(3.0f, 4.0f, 5.0f);
+  ASSERT_TRUE(abs(v1.sqrLength() - 50) < 1e-5);
+  ASSERT_TRUE(abs(v1.length() - sqrt(50)) < 1e-5);
+  ASSERT_TRUE(normalize(v1).equal(v1 / v1.length()));
+}
+
+TEST(vec_test, basic_vec64)
+{
+  typedef Vec3fX<details::Vec3Data<double> > Vec3f64;
+  Vec3f64 v1(1.0, 2.0, 3.0);
+  ASSERT_TRUE(v1[0] == 1.0);
+  ASSERT_TRUE(v1[1] == 2.0);
+  ASSERT_TRUE(v1[2] == 3.0);
+
+  Vec3f64 v2 = v1;
+  Vec3f64 v3(3.3, 4.3, 5.3);
+  v1 += v3;
+  ASSERT_TRUE(v1.equal(v2 + v3));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2 - v3));
+  v1 += v3;
+
+  v1 *= v3;
+  ASSERT_TRUE(v1.equal(v2 * v3));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2 / v3));
+  v1 *= v3;
+
+  v1 *= 2.0;
+  ASSERT_TRUE(v1.equal(v2 * 2.0));
+  v1 /= 2.0;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= 2.0;
+  ASSERT_TRUE(v1.equal(v2 / 2.0));
+  v1 *= 2.0;
+
+  v1 += 2.0;
+  ASSERT_TRUE(v1.equal(v2 + 2.0));
+  v1 -= 2.0;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= 2.0;
+  ASSERT_TRUE(v1.equal(v2 - 2.0));
+  v1 += 2.0;
+
+  ASSERT_TRUE((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0)));
+
+  v1 = Vec3f64(1.0, 2.0, 3.0);
+  v2 = Vec3f64(3.0, 4.0, 5.0);
+  ASSERT_TRUE((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+  ASSERT_TRUE(abs(v1.dot(v2) - 26) < 1e-5);
+
+  v1 = Vec3f64(3.0, 4.0, 5.0);
+  ASSERT_TRUE(abs(v1.sqrLength() - 50) < 1e-5);
+  ASSERT_TRUE(abs(v1.length() - sqrt(50)) < 1e-5);
+  ASSERT_TRUE(normalize(v1).equal(v1 / v1.length()));
+
+
+  v1 = Vec3f64(1.0, 2.0, 3.0);
+  v2 = Vec3f64(3.0, 4.0, 5.0);
+  ASSERT_TRUE((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+  ASSERT_TRUE(v1.dot(v2) == 26);
+}
+
+
+TEST(vec_test, sse_vec32)
+{
+  typedef Vec3fX<details::sse_meta_f4> Vec3f32;
+  Vec3f32 v1(1.0f, 2.0f, 3.0f);
+  ASSERT_TRUE(v1[0] == 1.0f);
+  ASSERT_TRUE(v1[1] == 2.0f);
+  ASSERT_TRUE(v1[2] == 3.0f);
+
+  Vec3f32 v2 = v1;
+  Vec3f32 v3(3.3f, 4.3f, 5.3f);
+  v1 += v3;
+  ASSERT_TRUE(v1.equal(v2 + v3));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2 - v3));
+  v1 += v3;
+
+  v1 *= v3;
+  ASSERT_TRUE(v1.equal(v2 * v3));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2 / v3));
+  v1 *= v3;
+
+  v1 *= 2.0f;
+  ASSERT_TRUE(v1.equal(v2 * 2.0f));
+  v1 /= 2.0f;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= 2.0f;
+  ASSERT_TRUE(v1.equal(v2 / 2.0f));
+  v1 *= 2.0f;
+
+  v1 += 2.0f;
+  ASSERT_TRUE(v1.equal(v2 + 2.0f));
+  v1 -= 2.0f;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= 2.0f;
+  ASSERT_TRUE(v1.equal(v2 - 2.0f));
+  v1 += 2.0f;
+  
+  ASSERT_TRUE((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f)));
+
+  v1 = Vec3f32(1.0f, 2.0f, 3.0f);
+  v2 = Vec3f32(3.0f, 4.0f, 5.0f);
+  ASSERT_TRUE((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f)));
+  ASSERT_TRUE(abs(v1.dot(v2) - 26) < 1e-5);
+
+  v1 = Vec3f32(3.0f, 4.0f, 5.0f);
+  ASSERT_TRUE(abs(v1.sqrLength() - 50) < 1e-5);
+  ASSERT_TRUE(abs(v1.length() - sqrt(50)) < 1e-5);
+  ASSERT_TRUE(normalize(v1).equal(v1 / v1.length()));
+}
+
+TEST(vec_test, sse_vec64)
+{
+  typedef Vec3fX<details::sse_meta_d4> Vec3f64;
+  Vec3f64 v1(1.0, 2.0, 3.0);
+  ASSERT_TRUE(v1[0] == 1.0);
+  ASSERT_TRUE(v1[1] == 2.0);
+  ASSERT_TRUE(v1[2] == 3.0);
+
+  Vec3f64 v2 = v1;
+  Vec3f64 v3(3.3, 4.3, 5.3);
+  v1 += v3;
+  ASSERT_TRUE(v1.equal(v2 + v3));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= v3;
+  ASSERT_TRUE(v1.equal(v2 - v3));
+  v1 += v3;
+
+  v1 *= v3;
+  ASSERT_TRUE(v1.equal(v2 * v3));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= v3;
+  ASSERT_TRUE(v1.equal(v2 / v3));
+  v1 *= v3;
+
+  v1 *= 2.0;
+  ASSERT_TRUE(v1.equal(v2 * 2.0));
+  v1 /= 2.0;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 /= 2.0;
+  ASSERT_TRUE(v1.equal(v2 / 2.0));
+  v1 *= 2.0;
+
+  v1 += 2.0;
+  ASSERT_TRUE(v1.equal(v2 + 2.0));
+  v1 -= 2.0;
+  ASSERT_TRUE(v1.equal(v2));
+  v1 -= 2.0;
+  ASSERT_TRUE(v1.equal(v2 - 2.0));
+  v1 += 2.0;
+
+  ASSERT_TRUE((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0)));
+
+  v1 = Vec3f64(1.0, 2.0, 3.0);
+  v2 = Vec3f64(3.0, 4.0, 5.0);
+  ASSERT_TRUE((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+  ASSERT_TRUE(abs(v1.dot(v2) - 26) < 1e-5);
+
+  v1 = Vec3f64(3.0, 4.0, 5.0);
+  ASSERT_TRUE(abs(v1.sqrLength() - 50) < 1e-5);
+  ASSERT_TRUE(abs(v1.length() - sqrt(50)) < 1e-5);
+  ASSERT_TRUE(normalize(v1).equal(v1 / v1.length()));
+
+
+  v1 = Vec3f64(1.0, 2.0, 3.0);
+  v2 = Vec3f64(3.0, 4.0, 5.0);
+  ASSERT_TRUE((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+  ASSERT_TRUE(v1.dot(v2) == 26);
+}
+
+TEST(mat_test, sse_mat32_consistent)
+{
+  typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
+  typedef Vec3fX<details::sse_meta_f4> Vec3f32SSE;
+
+  typedef Matrix3fX<details::Matrix3Data<float> > Matrix3f32;
+  typedef Matrix3fX<details::sse_meta_f12> Matrix3f32SSE;
+
+  Vec3f32 v1(1, 2, 3);
+  Vec3f32SSE v2(1, 2, 3);
+
+  Matrix3f32 m1(-1, 3, -3, 0, -6, 6, -5, -3, 1);
+  Matrix3f32SSE m2(-1, 3, -3, 0, -6, 6, -5, -3, 1);
+
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      ASSERT_TRUE((m1(i, j) - m2(i, j) < 1e-1));
+  
+  Matrix3f32 m3(transpose(m1));
+  Matrix3f32SSE m4(transpose(m2));
+        
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      ASSERT_TRUE((m3(i, j) - m4(i, j) < 1e-1));
+
+  m3 = m1; m3.transpose();
+  m4 = m2; m4.transpose();
+
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      ASSERT_TRUE((m3(i, j) - m4(i, j) < 1e-1));
+
+  m3 = inverse(m1);
+  m4 = inverse(m2);
+  
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      ASSERT_TRUE((m3(i, j) - m4(i, j) < 1e-1));
+
+  m3 = m1; m3.inverse();
+  m4 = m2; m4.inverse();
+
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      ASSERT_TRUE((m3(i, j) - m4(i, j) < 1e-1));
+}
+
+TEST(vec_test, sse_vec32_consistent)
+{
+  typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
+  typedef Vec3fX<details::sse_meta_f4> Vec3f32SSE;
+
+  Vec3f32 v1(3.4f, 4.2f, 10.5f), v2(3.1f, 0.1f, -50.4f);
+  Vec3f32SSE v3(3.4f, 4.2f, 10.5f), v4(3.1f, 0.1f, -50.4f);
+  Vec3f32 v12 = v1 + v2;
+  Vec3f32SSE v34 = v3 + v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - v2;
+  v34 = v3 - v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * v2;
+  v34 = v3 * v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / v2;
+  v34 = v3 / v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  float t = 1234.433f;
+  v12 = v1 + t;
+  v34 = v3 + t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - t;
+  v34 = v3 - t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * t;
+  v34 = v3 * t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / t;
+  v34 = v3 / t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12 += v2;
+  v34 = v3; v34 += v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= v2;
+  v34 = v3; v34 -= v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= v2;
+  v34 = v3; v34 *= v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= v2;
+  v34 = v3; v34 /= v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 += t;
+  v34 = v3; v34 += t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= t;
+  v34 = v3; v34 -= t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= t;
+  v34 = v3; v34 *= t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= t;
+  v34 = v3; v34 /= t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = -v1;
+  v34 = -v3;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1.cross(v2);
+  v34 = v3.cross(v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  ASSERT_TRUE(abs(v1.dot(v2) - v3.dot(v4)) < 1e-5);
+
+  v12 = min(v1, v2);
+  v34 = min(v3, v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = max(v1, v2);
+  v34 = max(v3, v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = abs(v2);
+  v34 = abs(v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  Vec3f32 delta1(1e-9f, 1e-9f, 1e-9f);
+  Vec3f32SSE delta2(1e-9f, 1e-9f, 1e-9f);
+  ASSERT_TRUE((v1 + delta1).equal(v1));
+  ASSERT_TRUE((v3 + delta2).equal(v3));
+
+  ASSERT_TRUE(abs(v1.length() - v3.length()) < 1e-5);
+  ASSERT_TRUE(abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ 
+  v12 = v1; v12.negate();
+  v34 = v3; v34.negate();
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12.normalize();
+  v34 = v3; v34.normalize();
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  
+  v12 = normalize(v1);
+  v34 = normalize(v3);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);  
+}
+
+TEST(vec_test, sse_vec64_consistent)
+{
+  typedef Vec3fX<details::Vec3Data<double> > Vec3f64;
+  typedef Vec3fX<details::sse_meta_d4> Vec3f64SSE;
+
+  Vec3f64 v1(3.4, 4.2, 10.5), v2(3.1, 0.1, -50.4);
+  Vec3f64SSE v3(3.4, 4.2, 10.5), v4(3.1, 0.1, -50.4);
+  Vec3f64 v12 = v1 + v2;
+  Vec3f64SSE v34 = v3 + v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - v2;
+  v34 = v3 - v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * v2;
+  v34 = v3 * v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / v2;
+  v34 = v3 / v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  double t = 1234.433;
+  v12 = v1 + t;
+  v34 = v3 + t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - t;
+  v34 = v3 - t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * t;
+  v34 = v3 * t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / t;
+  v34 = v3 / t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12 += v2;
+  v34 = v3; v34 += v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= v2;
+  v34 = v3; v34 -= v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= v2;
+  v34 = v3; v34 *= v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= v2;
+  v34 = v3; v34 /= v4;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 += t;
+  v34 = v3; v34 += t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= t;
+  v34 = v3; v34 -= t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= t;
+  v34 = v3; v34 *= t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= t;
+  v34 = v3; v34 /= t;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = -v1;
+  v34 = -v3;
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1.cross(v2);
+  v34 = v3.cross(v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  ASSERT_TRUE(abs(v1.dot(v2) - v3.dot(v4)) < 1e-5);
+
+  v12 = min(v1, v2);
+  v34 = min(v3, v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  v12 = max(v1, v2);
+  v34 = max(v3, v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = abs(v2);
+  v34 = abs(v4);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  Vec3f64 delta1(1e-15, 1e-15, 1e-15);
+  Vec3f64SSE delta2(1e-15, 1e-15, 1e-15);
+  ASSERT_TRUE((v1 + delta1).equal(v1));
+  ASSERT_TRUE((v3 + delta2).equal(v3));
+
+  ASSERT_TRUE(abs(v1.length() - v3.length()) < 1e-5);
+  ASSERT_TRUE(abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ 
+  v12 = v1; v12.negate();
+  v34 = v3; v34.negate();
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12.normalize();
+  v34 = v3; v34.normalize();
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);
+  
+  v12 = normalize(v1);
+  v34 = normalize(v3);
+  ASSERT_TRUE(abs(v12[0] - v34[0]) < 1e-5);
+  ASSERT_TRUE(abs(v12[1] - v34[1]) < 1e-5);
+  ASSERT_TRUE(abs(v12[2] - v34[2]) < 1e-5);  
+}
+
+TEST(morton_test, morton)
+{
+  AABB bbox(Vec3f(0, 0, 0), Vec3f(1000, 1000, 1000));
+  morton_functor<boost::dynamic_bitset<> > F1(bbox, 10);
+  morton_functor<boost::dynamic_bitset<> > F2(bbox, 20);
+  morton_functor<FCL_UINT64> F3(bbox);
+  morton_functor<FCL_UINT32> F4(bbox);
+
+  Vec3f p(254, 873, 674);
+  std::cout << std::hex << F1(p).to_ulong() << std::endl;
+  std::cout << std::hex << F2(p).to_ulong() << std::endl;
+  std::cout << std::hex << F3(p) << std::endl;
+  std::cout << std::hex << F4(p) << std::endl;
+  
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/trunk/fcl/test/test_fcl_octomap.cpp b/trunk/fcl/test/test_fcl_octomap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..13bcfcebf6740a3869a59efc3c01e8a15d1e256c
--- /dev/null
+++ b/trunk/fcl/test/test_fcl_octomap.cpp
@@ -0,0 +1,743 @@
+/*
+ * 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/octree.h"
+#include "fcl/traversal/traversal_node_octree.h"
+#include "fcl/broadphase/broadphase.h"
+#include "fcl/shape/geometric_shape_to_BVH_model.h"
+#include "fcl/math/transform.h"
+#include "test_fcl_utility.h"
+#include <gtest/gtest.h>
+
+using namespace fcl;
+
+struct TStruct
+{
+  std::vector<double> records;
+  double overall_time;
+
+  TStruct() { overall_time = 0; }
+  
+  void push_back(double t)
+  {
+    records.push_back(t);
+    overall_time += t;
+  }
+};
+
+/// @brief Generate environment with 3 * n objects: n spheres, n boxes and n cylinders
+void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
+
+/// @brief Generate environment with 3 * n objects: n spheres, n boxes and n cylinders, all in mesh
+void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n);
+
+/// @brief Generate boxes from the octomap
+void generateBoxesFromOctomap(std::vector<CollisionObject*>& env, OcTree& tree);
+
+/// @brief Generate boxes from the octomap
+void generateBoxesFromOctomapMesh(std::vector<CollisionObject*>& env, OcTree& tree);
+
+/// @brief Generate an octree
+octomap::OcTree* generateOcTree();
+
+/// @brief Octomap collision with an environment with 3 * env_size objects
+void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap);
+
+/// @brief Octomap collision with an environment with 3 * env_size objects, compute cost
+void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap);
+
+/// @brief Octomap distance with an environment with 3 * env_size objects
+void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap);
+
+
+template<typename BV>
+void octomap_collision_test_BVH(std::size_t n, bool exhaustive);
+
+
+template<typename BV>
+void octomap_distance_test_BVH(std::size_t n);
+
+TEST(test_octomap_cost, cost)
+{
+  octomap_cost_test(200, 100, 10, false, false);
+  octomap_cost_test(200, 1000, 10, false, false);
+}
+
+TEST(test_octomap_cost, cost_mesh)
+{
+  octomap_cost_test(200, 100, 10, true, false);
+  octomap_cost_test(200, 1000, 10, true, false);
+}
+
+TEST(test_octomap, collision)
+{
+  octomap_collision_test(200, 100, false, 10, false, false);
+  octomap_collision_test(200, 1000, false, 10, false, false);
+  octomap_collision_test(200, 100, true, 1, false, false);
+  octomap_collision_test(200, 1000, true, 1, false, false);
+}
+
+TEST(test_octomap, collision_mesh)
+{
+  octomap_collision_test(200, 100, false, 10, true, true);
+  octomap_collision_test(200, 1000, false, 10, true, true);
+  octomap_collision_test(200, 100, true, 1, true, true);
+  octomap_collision_test(200, 1000, true, 1, true, true);
+}
+
+TEST(test_octomap, collision_mesh_octomap_box)
+{
+  octomap_collision_test(200, 100, false, 10, true, false);
+  octomap_collision_test(200, 1000, false, 10, true, false);
+  octomap_collision_test(200, 100, true, 1, true, false);
+  octomap_collision_test(200, 1000, true, 1, true, false);
+}
+
+TEST(test_octomap, distance)
+{
+  octomap_distance_test(200, 100, false, false);
+  octomap_distance_test(200, 1000, false, false);
+}
+
+TEST(test_octomap, distance_mesh)
+{
+  octomap_distance_test(200, 100, true, true);
+  octomap_distance_test(200, 1000, true, true);
+}
+
+TEST(test_octomap, distance_mesh_octomap_box)
+{
+  octomap_distance_test(200, 100, true, false);
+  octomap_distance_test(200, 1000, true, false);
+}
+
+
+TEST(test_octomap_bvh_obb, collision_obb)
+{
+  octomap_collision_test_BVH<OBB>(5, false);
+  octomap_collision_test_BVH<OBB>(5, true);
+}
+
+TEST(test_octomap_bvh_rss_d, distance_rss)
+{
+  octomap_distance_test_BVH<RSS>(5);
+}
+
+TEST(test_octomap_bvh_obb_d, distance_obb)
+{
+  octomap_distance_test_BVH<OBBRSS>(5);
+}
+
+TEST(test_octomap_bvh_kios_d, distance_kios)
+{
+  octomap_distance_test_BVH<kIOS>(5);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+template<typename BV>
+void octomap_collision_test_BVH(std::size_t n, bool exhaustive)
+{
+  std::vector<Vec3f> p1;
+  std::vector<Triangle> t1;
+  loadOBJFile("../test/env.obj", p1, t1);
+  BVHModel<BV>* m1 = new BVHModel<BV>();
+  boost::shared_ptr<CollisionGeometry> m1_ptr(m1);
+
+  m1->beginModel();
+  m1->addSubModel(p1, t1);
+  m1->endModel();
+
+  OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
+  boost::shared_ptr<CollisionGeometry> tree_ptr(tree);
+
+  std::vector<Transform3f> transforms;
+  FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10};
+
+  generateRandomTransforms(extents, transforms, n);
+  
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Transform3f tf(transforms[i]);
+
+    CollisionObject obj1(m1_ptr, tf);
+    CollisionObject obj2(tree_ptr, tf);
+    CollisionData cdata;
+    if(exhaustive) cdata.request.num_max_contacts = 100000;
+
+    defaultCollisionFunction(&obj1, &obj2, &cdata);
+
+
+    std::vector<CollisionObject*> boxes;
+    generateBoxesFromOctomap(boxes, *tree);
+    for(std::size_t j = 0; j < boxes.size(); ++j)
+      boxes[j]->setTransform(tf * boxes[j]->getTransform());
+  
+    DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
+    manager->registerObjects(boxes);
+    manager->setup();
+
+    CollisionData cdata2;
+    if(exhaustive) cdata2.request.num_max_contacts = 100000;
+    manager->collide(&obj1, &cdata2, defaultCollisionFunction);
+
+    for(std::size_t j = 0; j < boxes.size(); ++j)
+      delete boxes[j];
+    delete manager;
+
+    if(exhaustive)
+    {
+      std::cout << cdata.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
+      ASSERT_TRUE(cdata.result.numContacts() == cdata2.result.numContacts());
+    }
+    else
+    {
+      std::cout << (cdata.result.numContacts() > 0) << " " << (cdata2.result.numContacts() > 0) << std::endl;
+      ASSERT_TRUE((cdata.result.numContacts() > 0) == (cdata2.result.numContacts() > 0));
+    }
+  }
+}
+
+
+template<typename BV>
+void octomap_distance_test_BVH(std::size_t n)
+{
+  std::vector<Vec3f> p1;
+  std::vector<Triangle> t1;
+  loadOBJFile("../test/env.obj", p1, t1);
+  BVHModel<BV>* m1 = new BVHModel<BV>();
+  boost::shared_ptr<CollisionGeometry> m1_ptr(m1);
+
+  m1->beginModel();
+  m1->addSubModel(p1, t1);
+  m1->endModel();
+
+  OcTree* tree = new OcTree(boost::shared_ptr<octomap::OcTree>(generateOcTree()));
+  boost::shared_ptr<CollisionGeometry> tree_ptr(tree);
+
+  std::vector<Transform3f> transforms;
+  FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10};
+
+  generateRandomTransforms(extents, transforms, n);
+  
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Transform3f tf(transforms[i]);
+
+    CollisionObject obj1(m1_ptr, tf);
+    CollisionObject obj2(tree_ptr, tf);
+    DistanceData cdata;
+    FCL_REAL dist1 = std::numeric_limits<FCL_REAL>::max();
+    defaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
+
+
+    std::vector<CollisionObject*> boxes;
+    generateBoxesFromOctomap(boxes, *tree);
+    for(std::size_t j = 0; j < boxes.size(); ++j)
+      boxes[j]->setTransform(tf * boxes[j]->getTransform());
+  
+    DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
+    manager->registerObjects(boxes);
+    manager->setup();
+
+    DistanceData cdata2;
+    manager->distance(&obj1, &cdata2, defaultDistanceFunction);
+    FCL_REAL dist2 = cdata2.result.min_distance;
+
+    for(std::size_t j = 0; j < boxes.size(); ++j)
+      delete boxes[j];
+    delete manager;
+
+    std::cout << dist1 << " " << dist2 << std::endl;
+    ASSERT_TRUE(std::abs(dist1 - dist2) < 0.001);
+  }
+}
+
+
+void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap)
+{
+  std::vector<CollisionObject*> env;
+  if(use_mesh)
+    generateEnvironmentsMesh(env, env_scale, env_size);
+  else
+    generateEnvironments(env, env_scale, env_size);
+
+  OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
+  CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));
+
+  DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
+  manager->registerObjects(env);
+  manager->setup();
+  
+  CollisionData cdata;
+  cdata.request.enable_cost = true;
+  cdata.request.num_max_cost_sources = num_max_cost_sources;
+
+  TStruct t1;
+  Timer timer1;
+  timer1.start();
+  manager->octree_as_geometry_collide = false;
+  manager->octree_as_geometry_distance = false;
+  manager->collide(&tree_obj, &cdata, defaultCollisionFunction);
+  timer1.stop();
+  t1.push_back(timer1.getElapsedTime());
+
+  CollisionData cdata3;
+  cdata3.request.enable_cost = true;
+  cdata3.request.num_max_cost_sources = num_max_cost_sources;
+
+  TStruct t3;
+  Timer timer3;
+  timer3.start();
+  manager->octree_as_geometry_collide = true;
+  manager->octree_as_geometry_distance = true;
+  manager->collide(&tree_obj, &cdata3, defaultCollisionFunction);
+  timer3.stop();
+  t3.push_back(timer3.getElapsedTime());
+
+  TStruct t2;
+  Timer timer2;
+  timer2.start();
+  std::vector<CollisionObject*> boxes;
+  if(use_mesh_octomap)
+    generateBoxesFromOctomapMesh(boxes, *tree);
+  else
+    generateBoxesFromOctomap(boxes, *tree);
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+  
+  timer2.start();
+  DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
+  manager2->registerObjects(boxes);
+  manager2->setup();
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+
+
+  CollisionData cdata2;
+  cdata2.request.enable_cost = true;
+  cdata3.request.num_max_cost_sources = num_max_cost_sources;
+
+  timer2.start();
+  manager->collide(manager2, &cdata2, defaultCollisionFunction);
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+
+  std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
+  std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl;
+
+  {
+    std::vector<CostSource> cost_sources;
+    cdata.result.getCostSources(cost_sources);
+    for(std::size_t i = 0; i < cost_sources.size(); ++i)
+    {
+      std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
+    }
+
+    std::cout << std::endl;
+
+    cdata3.result.getCostSources(cost_sources);
+    for(std::size_t i = 0; i < cost_sources.size(); ++i)
+    {
+      std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
+    }
+
+    std::cout << std::endl;
+
+    cdata2.result.getCostSources(cost_sources);
+    for(std::size_t i = 0; i < cost_sources.size(); ++i)
+    {
+      std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl;
+    }
+
+    std::cout << std::endl;
+
+  }
+
+  if(use_mesh) ASSERT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
+  else ASSERT_TRUE(cdata.result.numContacts() >= cdata2.result.numContacts());
+
+  delete manager;
+  delete manager2;
+  for(std::size_t i = 0; i < boxes.size(); ++i)
+    delete boxes[i];
+
+  std::cout << "collision cost" << std::endl;
+  std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
+  std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
+  std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
+  std::cout << "  a) to boxes: " << t2.records[0] << std::endl;
+  std::cout << "  b) structure init: " << t2.records[1] << std::endl;
+  std::cout << "  c) collision: " << t2.records[2] << std::endl;
+  std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
+}
+
+
+void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap)
+{
+  // srand(1);
+  std::vector<CollisionObject*> env;
+  if(use_mesh)
+    generateEnvironmentsMesh(env, env_scale, env_size);
+  else
+    generateEnvironments(env, env_scale, env_size);
+
+  OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
+  CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));
+
+  DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
+  manager->registerObjects(env);
+  manager->setup();
+  
+  CollisionData cdata;
+  if(exhaustive) cdata.request.num_max_contacts = 100000;
+  else cdata.request.num_max_contacts = num_max_contacts;
+
+  TStruct t1;
+  Timer timer1;
+  timer1.start();
+  manager->octree_as_geometry_collide = false;
+  manager->octree_as_geometry_distance = false;
+  manager->collide(&tree_obj, &cdata, defaultCollisionFunction);
+  timer1.stop();
+  t1.push_back(timer1.getElapsedTime());
+
+  CollisionData cdata3;
+  if(exhaustive) cdata3.request.num_max_contacts = 100000;
+  else cdata3.request.num_max_contacts = num_max_contacts;
+
+  TStruct t3;
+  Timer timer3;
+  timer3.start();
+  manager->octree_as_geometry_collide = true;
+  manager->octree_as_geometry_distance = true;
+  manager->collide(&tree_obj, &cdata3, defaultCollisionFunction);
+  timer3.stop();
+  t3.push_back(timer3.getElapsedTime());
+
+  TStruct t2;
+  Timer timer2;
+  timer2.start();
+  std::vector<CollisionObject*> boxes;
+  if(use_mesh_octomap)
+    generateBoxesFromOctomapMesh(boxes, *tree);
+  else
+    generateBoxesFromOctomap(boxes, *tree);
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+  
+  timer2.start();
+  DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
+  manager2->registerObjects(boxes);
+  manager2->setup();
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+
+
+  CollisionData cdata2;
+  if(exhaustive) cdata2.request.num_max_contacts = 100000;
+  else cdata2.request.num_max_contacts = num_max_contacts;
+
+  timer2.start();
+  manager->collide(manager2, &cdata2, defaultCollisionFunction);
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+
+  std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl;
+  if(exhaustive)
+  {
+    if(use_mesh) ASSERT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
+    else ASSERT_TRUE(cdata.result.numContacts() == cdata2.result.numContacts());
+  }
+  else
+  {
+    if(use_mesh) ASSERT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0));
+    else ASSERT_TRUE((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); // because AABB return collision when two boxes contact
+  }
+
+  delete manager;
+  delete manager2;
+  for(size_t i = 0; i < boxes.size(); ++i)
+    delete boxes[i];
+
+  if(exhaustive) std::cout << "exhaustive collision" << std::endl;
+  else std::cout << "non exhaustive collision" << std::endl;
+  std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
+  std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
+  std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
+  std::cout << "  a) to boxes: " << t2.records[0] << std::endl;
+  std::cout << "  b) structure init: " << t2.records[1] << std::endl;
+  std::cout << "  c) collision: " << t2.records[2] << std::endl;
+  std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
+}
+
+
+void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap)
+{
+  // srand(1);
+  std::vector<CollisionObject*> env;
+  if(use_mesh)
+    generateEnvironmentsMesh(env, env_scale, env_size);
+  else
+    generateEnvironments(env, env_scale, env_size);
+
+  OcTree* tree = new OcTree(boost::shared_ptr<const octomap::OcTree>(generateOcTree()));
+  CollisionObject tree_obj((boost::shared_ptr<CollisionGeometry>(tree)));
+
+  DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager();
+  manager->registerObjects(env);
+  manager->setup();
+  
+  DistanceData cdata;
+  TStruct t1;
+  Timer timer1;
+  timer1.start();
+  manager->octree_as_geometry_collide = false;
+  manager->octree_as_geometry_distance = false;
+  manager->distance(&tree_obj, &cdata, defaultDistanceFunction);
+  timer1.stop();
+  t1.push_back(timer1.getElapsedTime());
+
+  
+  DistanceData cdata3;
+  TStruct t3;
+  Timer timer3;
+  timer3.start();
+  manager->octree_as_geometry_collide = true;
+  manager->octree_as_geometry_distance = true;
+  manager->distance(&tree_obj, &cdata3, defaultDistanceFunction);
+  timer3.stop();
+  t3.push_back(timer3.getElapsedTime());
+  
+
+  TStruct t2;
+  Timer timer2;
+  timer2.start();
+  std::vector<CollisionObject*> boxes;
+  if(use_mesh_octomap)
+    generateBoxesFromOctomapMesh(boxes, *tree);
+  else
+    generateBoxesFromOctomap(boxes, *tree);
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+  
+  timer2.start();
+  DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager();
+  manager2->registerObjects(boxes);
+  manager2->setup();
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+
+
+  DistanceData cdata2;
+
+  timer2.start();
+  manager->distance(manager2, &cdata2, defaultDistanceFunction);
+  timer2.stop();
+  t2.push_back(timer2.getElapsedTime());
+
+  std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl;
+
+  if(cdata.result.min_distance < 0)
+    ASSERT_TRUE(cdata2.result.min_distance <= 0);
+  else
+    ASSERT_TRUE(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3);
+
+  delete manager;
+  delete manager2;
+  for(size_t i = 0; i < boxes.size(); ++i)
+    delete boxes[i];
+
+
+  std::cout << "1) octomap overall time: " << t1.overall_time << std::endl;
+  std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl;
+  std::cout << "2) boxes overall time: " << t2.overall_time << std::endl;
+  std::cout << "  a) to boxes: " << t2.records[0] << std::endl;
+  std::cout << "  b) structure init: " << t2.records[1] << std::endl;
+  std::cout << "  c) distance: " << t2.records[2] << std::endl;
+  std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl;
+}
+
+
+
+void generateBoxesFromOctomap(std::vector<CollisionObject*>& boxes, OcTree& tree)
+{
+  std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();
+
+  for(std::size_t i = 0; i < boxes_.size(); ++i)
+  {
+    FCL_REAL x = boxes_[i][0];
+    FCL_REAL y = boxes_[i][1];
+    FCL_REAL z = boxes_[i][2];
+    FCL_REAL size = boxes_[i][3];
+    FCL_REAL cost = boxes_[i][4];
+    FCL_REAL threshold = boxes_[i][5];
+
+    Box* box = new Box(size, size, size);
+    box->cost_density = cost;
+    box->threshold_occupied = threshold;
+    CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), Transform3f(Vec3f(x, y, z)));
+    boxes.push_back(obj);
+  }
+
+  std::cout << "boxes size: " << boxes.size() << std::endl;
+
+}
+
+void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
+{
+  FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
+  std::vector<Transform3f> transforms;
+
+  generateRandomTransforms(extents, transforms, n);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Box* box = new Box(5, 10, 20);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(box), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Sphere* sphere = new Sphere(30);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(sphere), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    Cylinder* cylinder = new Cylinder(10, 40);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(cylinder), transforms[i]));
+  }
+
+}
+
+
+void generateBoxesFromOctomapMesh(std::vector<CollisionObject*>& boxes, OcTree& tree)
+{
+  std::vector<boost::array<FCL_REAL, 6> > boxes_ = tree.toBoxes();
+
+  for(std::size_t i = 0; i < boxes_.size(); ++i)
+  {
+    FCL_REAL x = boxes_[i][0];
+    FCL_REAL y = boxes_[i][1];
+    FCL_REAL z = boxes_[i][2];
+    FCL_REAL size = boxes_[i][3];
+    FCL_REAL cost = boxes_[i][4];
+    FCL_REAL threshold = boxes_[i][5];
+
+    Box box(size, size, size);
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, box, Transform3f());
+    model->cost_density = cost;
+    model->threshold_occupied = threshold;
+    CollisionObject* obj = new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), Transform3f(Vec3f(x, y, z)));
+    boxes.push_back(obj);    
+  }
+
+  std::cout << "boxes size: " << boxes.size() << std::endl;
+}
+
+void generateEnvironmentsMesh(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
+{
+  FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
+  std::vector<Transform3f> transforms;
+
+  generateRandomTransforms(extents, transforms, n);
+  Box box(5, 10, 20);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, box, Transform3f());
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  Sphere sphere(30);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, sphere, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
+  }
+
+  generateRandomTransforms(extents, transforms, n);
+  Cylinder cylinder(10, 40);
+  for(std::size_t i = 0; i < n; ++i)
+  {
+    BVHModel<OBBRSS>* model = new BVHModel<OBBRSS>();
+    generateBVHModel(*model, cylinder, Transform3f(), 16, 16);
+    env.push_back(new CollisionObject(boost::shared_ptr<CollisionGeometry>(model), transforms[i]));
+  }
+}
+
+
+octomap::OcTree* generateOcTree()
+{
+  octomap::OcTree* tree = new octomap::OcTree(0.1);
+
+  // insert some measurements of occupied cells
+  for(int x = -20; x < 20; x++) 
+  {
+    for(int y = -20; y < 20; y++) 
+    {
+      for(int z = -20; z < 20; z++) 
+      {
+        tree->updateNode(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), true);
+      }
+    }
+  }
+
+  // insert some measurements of free cells
+  for(int x = -30; x < 30; x++) 
+  {
+    for(int y = -30; y < 30; y++) 
+    {
+      for(int z = -30; z < 30; z++) 
+      {
+        tree->updateNode(octomap::point3d(x*0.02 -1.0, y*0.02-1.0, z*0.02-1.0), false);
+      }
+    }
+  }
+
+  return tree;  
+}
+
+
diff --git a/trunk/fcl/test/test_fcl_shape_mesh_consistency.cpp b/trunk/fcl/test/test_fcl_shape_mesh_consistency.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..882002a600433d55d1217d95779a0155c7d82b96
--- /dev/null
+++ b/trunk/fcl/test/test_fcl_shape_mesh_consistency.cpp
@@ -0,0 +1,2195 @@
+/*
+ * 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/shape/geometric_shape_to_BVH_model.h"
+#include "fcl/distance.h"
+#include "fcl/collision.h"
+#include "test_fcl_utility.h"
+#include <gtest/gtest.h>
+
+
+using namespace fcl;
+
+GJKSolver_libccd solver1;
+GJKSolver_indep solver2;
+
+FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10};
+
+TEST(consistency_distance, spheresphere_libccd)
+{
+  Sphere s1(20);
+  Sphere s2(20);
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(50, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);  
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  }
+
+  pose.setTranslation(Vec3f(40.1, 0, 0));
+
+  res.clear(), res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver1, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
+TEST(consistency_distance, boxbox_libccd)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f());
+  generateBVHModel(s2_rss, s2, Transform3f());
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(50, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver1, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  }
+
+  pose.setTranslation(Vec3f(15.1, 0, 0));
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver1, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
+TEST(consistency_distance, cylindercylinder_libccd)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(20, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver1, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  }
+  
+  pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( 
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver1, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
+TEST(consistency_distance, conecone_libccd)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(20, 0, 0));
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver1, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  }
+  
+  pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( 
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver1, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver1, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver1, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver1, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
+
+TEST(consistency_distance, spheresphere_GJK)
+{
+  Sphere s1(20);
+  Sphere s2(20);
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(50, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);  
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  }
+
+  pose.setTranslation(Vec3f(40.1, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
+
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4);
+  }
+}
+
+TEST(consistency_distance, boxbox_GJK)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f());
+  generateBVHModel(s2_rss, s2, Transform3f());
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(50, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01);
+  }
+
+  pose.setTranslation(Vec3f(15.1, 0, 0));
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
+TEST(consistency_distance, cylindercylinder_GJK)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+  
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(20, 0, 0));
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
+      std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
+    else
+      ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
+      std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
+    else
+      ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05)
+      std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl;
+    else
+      ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  }
+  
+  pose.setTranslation(Vec3f(10.1, 0, 0));
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
+TEST(consistency_distance, conecone_GJK)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  BVHModel<RSS> s1_rss;
+  BVHModel<RSS> s2_rss;
+
+  generateBVHModel(s1_rss, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_rss, s2, Transform3f(), 16, 16);
+
+  DistanceRequest request;
+  DistanceResult res, res1;
+
+  Transform3f pose;
+
+  pose.setTranslation(Vec3f(20, 0, 0));
+
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05);
+  }
+  
+  pose.setTranslation(Vec3f(10.1, 0, 0));
+  
+  res.clear(); res1.clear();
+  distance(&s1, Transform3f(), &s2, pose, &solver2, request, res);
+  distance(&s1_rss, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  res1.clear();
+  distance(&s1, Transform3f(), &s2_rss, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+  res1.clear();
+  distance(&s1_rss, Transform3f(), &s2, pose, &solver2, request, res1);
+  ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  
+  for(std::size_t i = 0; i < 10; ++i)
+  {
+    Transform3f t;
+    generateRandomTransform(extents, t);
+    
+    Transform3f pose1(t);
+    Transform3f pose2 = t * pose;
+
+    res.clear(); res1.clear();
+    distance(&s1, pose1, &s2, pose2, &solver2, request, res);
+    distance(&s1_rss, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1, pose1, &s2_rss, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+
+    res1.clear();
+    distance(&s1_rss, pose1, &s2, pose2, &solver2, request, res1);
+    ASSERT_TRUE(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2);
+  }
+}
+
+
+
+TEST(consistency_collision, spheresphere_libccd)
+{
+  Sphere s1(20);
+  Sphere s2(10);
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(40, 0, 0));
+  pose_aabb.setTranslation(Vec3f(40, 0, 0));
+  pose_obb.setTranslation(Vec3f(40, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  
+  pose.setTranslation(Vec3f(30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(30, 0, 0));
+  pose_obb.setTranslation(Vec3f(30, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  
+  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
+  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_collision, boxbox_libccd)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f());
+  generateBVHModel(s2_aabb, s2, Transform3f());
+  generateBVHModel(s1_obb, s1, Transform3f());
+  generateBVHModel(s2_obb, s2, Transform3f());
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(15.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(14.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+}
+
+TEST(consistency_collision, spherebox_libccd)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f());
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f());
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(22.4, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.4, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(22.51, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_collision, cylindercylinder_libccd)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  pose.setTranslation(Vec3f(9.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.99, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(10.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_collision, conecone_libccd)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  pose.setTranslation(Vec3f(9.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.9, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(10.1, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(0, 0, 9.9));
+  pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
+  pose_obb.setTranslation(Vec3f(0, 0, 9.9));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(0, 0, 10.1));
+  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
+  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver1, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+
+
+
+TEST(consistency_collision, spheresphere_GJK)
+{
+  Sphere s1(20);
+  Sphere s2(10);
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(40, 0, 0));
+  pose_aabb.setTranslation(Vec3f(40, 0, 0));
+  pose_obb.setTranslation(Vec3f(40, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  
+  pose.setTranslation(Vec3f(30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(30, 0, 0));
+  pose_obb.setTranslation(Vec3f(30, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  
+  pose.setTranslation(Vec3f(29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+
+  pose.setTranslation(Vec3f(-29.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+  pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(-30, 0, 0));
+  pose_aabb.setTranslation(Vec3f(-30, 0, 0));
+  pose_obb.setTranslation(Vec3f(-30, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_collision, boxbox_GJK)
+{
+  Box s1(20, 40, 50);
+  Box s2(10, 10, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f());
+  generateBVHModel(s2_aabb, s2, Transform3f());
+  generateBVHModel(s1_obb, s1, Transform3f());
+  generateBVHModel(s2_obb, s2, Transform3f());
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(15.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(15.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(15.01, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(14.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(14.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(14.99, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+}
+
+TEST(consistency_collision, spherebox_GJK)
+{
+  Sphere s1(20);
+  Box s2(5, 5, 5);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f());
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f());
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  // s2 is within s1
+  // both are shapes --> collision
+  // s1 is shape, s2 is mesh --> in collision
+  // s1 is mesh, s2 is shape --> collision free
+  // all are reasonable
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(22.4, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.4, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.4, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(22.51, 0, 0));
+  pose_aabb.setTranslation(Vec3f(22.51, 0, 0));
+  pose_obb.setTranslation(Vec3f(22.51, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_collision, cylindercylinder_GJK)
+{
+  Cylinder s1(5, 10);
+  Cylinder s2(5, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  pose.setTranslation(Vec3f(9.99, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.99, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.99, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(10.01, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.01, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.01, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+TEST(consistency_collision, conecone_GJK)
+{
+  Cone s1(5, 10);
+  Cone s2(5, 10);
+
+  BVHModel<AABB> s1_aabb;
+  BVHModel<AABB> s2_aabb;
+  BVHModel<OBB> s1_obb;
+  BVHModel<OBB> s2_obb;
+
+  generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16);
+  generateBVHModel(s1_obb, s1, Transform3f(), 16, 16);
+  generateBVHModel(s2_obb, s2, Transform3f(), 16, 16);
+
+  CollisionRequest request;
+  CollisionResult result;
+
+  bool res;
+
+  Transform3f pose, pose_aabb, pose_obb;
+
+  pose.setTranslation(Vec3f(9.9, 0, 0));
+  pose_aabb.setTranslation(Vec3f(9.9, 0, 0));
+  pose_obb.setTranslation(Vec3f(9.9, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(10.1, 0, 0));
+  pose_aabb.setTranslation(Vec3f(10.1, 0, 0));
+  pose_obb.setTranslation(Vec3f(10.1, 0, 0));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+
+  pose.setTranslation(Vec3f(0, 0, 9.9));
+  pose_aabb.setTranslation(Vec3f(0, 0, 9.9));
+  pose_obb.setTranslation(Vec3f(0, 0, 9.9));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_TRUE(res);
+
+  pose.setTranslation(Vec3f(0, 0, 10.1));
+  pose_aabb.setTranslation(Vec3f(0, 0, 10.1));
+  pose_obb.setTranslation(Vec3f(0, 0, 10.1));
+
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_aabb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s2, pose, &s1_obb, Transform3f(), &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+  result.clear();
+  res = (collide(&s1_aabb, Transform3f(), &s2, pose, &solver2, request, result) > 0);
+  ASSERT_FALSE(res);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+
+
+
+
+
diff --git a/trunk/fcl/test/test_fcl_utility.cpp b/trunk/fcl/test/test_fcl_utility.cpp
index ba988947a8881ad0d9e92d1890dd809c854f8b9e..e0a654a975ccf9196839cddef128087c65e5c542 100644
--- a/trunk/fcl/test/test_fcl_utility.cpp
+++ b/trunk/fcl/test/test_fcl_utility.cpp
@@ -2,10 +2,95 @@
 #include "fcl/collision.h"
 #include "fcl/distance.h"
 #include <cstdio>
+#include <cstddef>
 
 namespace fcl
 {
 
+
+Timer::Timer()
+{
+#ifdef _WIN32
+  QueryPerformanceFrequency(&frequency);
+  startCount.QuadPart = 0;
+  endCount.QuadPart = 0;
+#else
+  startCount.tv_sec = startCount.tv_usec = 0;
+  endCount.tv_sec = endCount.tv_usec = 0;
+#endif
+
+  stopped = 0;
+  startTimeInMicroSec = 0;
+  endTimeInMicroSec = 0;
+}
+
+
+Timer::~Timer()
+{
+}
+
+
+void Timer::start()
+{
+  stopped = 0; // reset stop flag
+#ifdef _WIN32
+  QueryPerformanceCounter(&startCount);
+#else
+  gettimeofday(&startCount, NULL);
+#endif
+}
+
+
+void Timer::stop()
+{
+  stopped = 1; // set timer stopped flag
+
+#ifdef _WIN32
+  QueryPerformanceCounter(&endCount);
+#else
+  gettimeofday(&endCount, NULL);
+#endif
+}
+
+
+double Timer::getElapsedTimeInMicroSec()
+{
+#ifdef _WIN32
+  if(!stopped)
+    QueryPerformanceCounter(&endCount);
+
+  startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart);
+  endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart);
+#else
+  if(!stopped)
+    gettimeofday(&endCount, NULL);
+
+  startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec;
+  endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec;
+#endif
+
+  return endTimeInMicroSec - startTimeInMicroSec;
+}
+
+
+double Timer::getElapsedTimeInMilliSec()
+{
+  return this->getElapsedTimeInMicroSec() * 0.001;
+}
+
+
+double Timer::getElapsedTimeInSec()
+{
+  return this->getElapsedTimeInMicroSec() * 0.000001;
+}
+
+
+double Timer::getElapsedTime()
+{
+  return this->getElapsedTimeInMilliSec();
+}
+
+
 FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax)
 {
   FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1);
@@ -155,7 +240,7 @@ void generateRandomTransforms(FCL_REAL extents[6], std::vector<Transform3f>& tra
 }
 
 
-void generateRandomTransforms(FCL_REAL extents[6], std::vector<Transform3f>& transforms, std::vector<Transform3f>& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n)
+void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector<Transform3f>& transforms, std::vector<Transform3f>& transforms2, std::size_t n)
 {
   transforms.resize(n);
   transforms2.resize(n);
diff --git a/trunk/fcl/test/test_fcl_utility.h b/trunk/fcl/test/test_fcl_utility.h
index bdbfadfbd2f877b888f42fd3d18d9ccc4f5bf745..5ca5bab3290e1394cb00eb0c197f32ae094a3736 100644
--- a/trunk/fcl/test/test_fcl_utility.h
+++ b/trunk/fcl/test/test_fcl_utility.h
@@ -41,9 +41,44 @@
 #include "fcl/collision_data.h"
 #include "fcl/collision_object.h"
 
+#ifdef _WIN32
+#include <windows.h>
+#else
+#include <sys/time.h>
+#endif
+
+
 namespace fcl
 {
 
+class Timer
+{
+public:
+  Timer();
+  ~Timer();
+
+  void start();                               ///< start timer
+  void stop();                                ///< stop the timer
+  double getElapsedTime();                    ///< get elapsed time in milli-second
+  double getElapsedTimeInSec();               ///< get elapsed time in second (same as getElapsedTime)
+  double getElapsedTimeInMilliSec();          ///< get elapsed time in milli-second
+  double getElapsedTimeInMicroSec();          ///< get elapsed time in micro-second
+
+private:
+  double startTimeInMicroSec;                 ///< starting time in micro-second
+  double endTimeInMicroSec;                   ///< ending time in micro-second
+  int stopped;                                ///< stop flag
+#ifdef _WIN32
+  LARGE_INTEGER frequency;                    ///< ticks per second
+  LARGE_INTEGER startCount;
+  LARGE_INTEGER endCount;
+#else
+  timeval startCount;
+  timeval endCount;
+#endif
+};
+
+
 /// @brief Load an obj mesh file
 void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles);