diff --git a/CMakeLists.txt b/CMakeLists.txt
index d07f9a44d..7fd094f57 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,6 +25,7 @@ endif (MSVC OR MSVC90 OR MSVC10)
 
 set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)
 set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules")
+include(FCLMacros)
 include(CompilerSettings)
 include(FCLVersion)
 include(GNUInstallDirs)
diff --git a/CMakeModules/FCLMacros.cmake b/CMakeModules/FCLMacros.cmake
new file mode 100644
index 000000000..e160e01bf
--- /dev/null
+++ b/CMakeModules/FCLMacros.cmake
@@ -0,0 +1,54 @@
+#===============================================================================
+# Appends items to a cached list.
+# Usage:
+#   fcl_append_to_cached_string(_string _cacheDesc [items...])
+#===============================================================================
+macro(fcl_append_to_cached_string _string _cacheDesc)
+  foreach(newItem ${ARGN})
+    set(${_string} "${${_string}}${newItem}" CACHE INTERNAL ${_cacheDesc} FORCE)
+  endforeach()
+endmacro()
+
+#===============================================================================
+# Get list of file names give list of full paths.
+# Usage:
+#   fcl_get_filename_components(_var _cacheDesc [items...])
+#===============================================================================
+macro(fcl_get_filename_components _var _cacheDesc _suffix_to_remove)
+  set(${_var} "" CACHE INTERNAL ${_cacheDesc} FORCE)
+  string(LENGTH ${_suffix_to_remove} suffix_length)
+  foreach(header ${ARGN})
+    string(LENGTH ${header} full_length)
+    string(
+      SUBSTRING
+      ${header}
+      ${suffix_length}
+      ${full_length}-${suffix_length}
+      header)
+      if(${header} MATCHES "/detail/")
+        continue()
+      endif()
+      if(${header} MATCHES "-impl.h")
+        continue()
+      endif()
+    fcl_append_to_cached_string(
+      ${_var}
+      ${_cacheDesc}"_HEADER_NAMES"
+      "${header}\;")
+  endforeach()
+endmacro()
+
+#===============================================================================
+# Generate header file list to a cached list.
+# Usage:
+#   fcl_generate_include_header_list(_var _target_dir _cacheDesc [headers...])
+#===============================================================================
+macro(fcl_generate_include_header_list _var _target_dir _cacheDesc)
+  set(${_var} "" CACHE INTERNAL ${_cacheDesc} FORCE)
+  foreach(header ${ARGN})
+    fcl_append_to_cached_string(
+      ${_var}
+      ${_cacheDesc}"_HEADERS"
+      "#include \"${_target_dir}${header}\"\n")
+  endforeach()
+endmacro()
diff --git a/include/fcl/CMakeLists.txt b/include/fcl/CMakeLists.txt
index 260325c69..b2a654edd 100644
--- a/include/fcl/CMakeLists.txt
+++ b/include/fcl/CMakeLists.txt
@@ -6,3 +6,34 @@ file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" FCL_CONFIG_IN_DIR)
 file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}" FCL_CONFIG_OUT_DIR)
 configure_file("${FCL_CONFIG_IN_DIR}/config.h.in" "${FCL_CONFIG_OUT_DIR}/config.h")
 install(FILES "${FCL_CONFIG_OUT_DIR}/config.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/fcl)
+
+file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" FCL_TOP_HEADER_IN_DIR)
+file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}" FCL_TOP_HEADER_OUT_DIR)
+configure_file("${FCL_CONFIG_IN_DIR}/config.h.in" "${FCL_CONFIG_OUT_DIR}/config.h")
+install(FILES "${FCL_CONFIG_OUT_DIR}/config.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/fcl)
+
+fcl_get_filename_components(
+  fcl_header_rel_paths
+  "fcl headers"
+  ${CMAKE_CURRENT_SOURCE_DIR}
+  ${HEADERS})
+fcl_get_filename_components(
+  fcl_config_header_rel_paths
+  "fcl configured headers"
+  ${CMAKE_CURRENT_BINARY_DIR}
+  ${CMAKE_CURRENT_BINARY_DIR}/config.h)
+list(INSERT fcl_header_rel_paths 0 ${fcl_config_header_rel_paths})
+
+fcl_generate_include_header_list(
+  fcl_headers
+  "fcl"
+  "fcl header relative paths"
+  ${fcl_header_rel_paths})
+configure_file(
+  ${CMAKE_CURRENT_SOURCE_DIR}/fcl.h.in
+  ${CMAKE_CURRENT_BINARY_DIR}/fcl.h)
+
+install(
+  FILES ${hdrs} ${CMAKE_CURRENT_BINARY_DIR}/fcl.h
+  DESTINATION include/fcl
+  COMPONENT headers)
diff --git a/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h
index 138dbff9b..3314c3317 100644
--- a/include/fcl/broadphase/broadphase_spatialhash.h
+++ b/include/fcl/broadphase/broadphase_spatialhash.h
@@ -43,6 +43,7 @@
 #include "fcl/math/bv/AABB.h"
 #include "fcl/broadphase/broadphase_collision_manager.h"
 #include "fcl/broadphase/detail/simple_hash_table.h"
+#include "fcl/broadphase/detail/sparse_hash_table.h"
 #include "fcl/broadphase/detail/spatial_hash.h"
 
 namespace fcl
diff --git a/include/fcl/fcl.h b/include/fcl/fcl.h.in
similarity index 96%
rename from include/fcl/fcl.h
rename to include/fcl/fcl.h.in
index 7b694db62..dee93b29c 100644
--- a/include/fcl/fcl.h
+++ b/include/fcl/fcl.h.in
@@ -34,9 +34,11 @@
 
 /** @author Jeongseok Lee <jslee02@gmail.com> */
 
+// Automatically generated file by cmake
+
 #ifndef FCL_FCL_H
 #define FCL_FCL_H
 
-// TODO(JS): fill necessary headers here
+${fcl_headers}
 
 #endif
diff --git a/include/fcl/math/motion/spline_motion.h b/include/fcl/math/motion/spline_motion.h
index 36e85e334..6f666bdba 100644
--- a/include/fcl/math/motion/spline_motion.h
+++ b/include/fcl/math/motion/spline_motion.h
@@ -42,6 +42,7 @@
 #include <vector>
 #include "fcl/math/geometry.h"
 #include "fcl/math/motion/motion_base.h"
+#include "fcl/math/detail/polysolver.h"
 
 namespace fcl
 {
@@ -399,7 +400,7 @@ S SplineMotion<S>::computeDWMax() const
 
   S roots[3];
 
-  int root_num = PolySolver<S>::solveCubic(da, roots);
+  int root_num = detail::PolySolver<S>::solveCubic(da, roots);
 
   S dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
   S dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
diff --git a/include/fcl/math/sampler/sampling.h b/include/fcl/math/sampler/sampling.h
deleted file mode 100644
index 829dfbf11..000000000
--- a/include/fcl/math/sampler/sampling.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2013-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2016, Open Source Robotics Foundation
- *  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 Open Source Robotics Foundation 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 */
-
-#ifndef FCL_MATH_SAMPLING_H
-#define FCL_MATH_SAMPLING_H
-
-#warning "This header has been deprecated in FCL 0.6. "
-
-#include "fcl/math/sampler/sampler_base.h"
-#include "fcl/math/sampler/sampler_r.h"
-#include "fcl/math/sampler/sampler_se2.h"
-#include "fcl/math/sampler/sampler_se2_disk.h"
-#include "fcl/math/sampler/sampler_se3_euler.h"
-#include "fcl/math/sampler/sampler_se3_euler_ball.h"
-#include "fcl/math/sampler/sampler_se3_quat.h"
-#include "fcl/math/sampler/sampler_se3_quat_ball.h"
-
-#endif
diff --git a/include/fcl/narrowphase/continuous_collision.h b/include/fcl/narrowphase/continuous_collision.h
index 5e635fc0a..766bb1eac 100644
--- a/include/fcl/narrowphase/continuous_collision.h
+++ b/include/fcl/narrowphase/continuous_collision.h
@@ -41,7 +41,10 @@
 #include <iostream>
 #include "fcl/object/collision_object.h"
 #include "fcl/object/continuous_collision_object.h"
+#include "fcl/narrowphase/detail/gjk_solver_indep.h"
+#include "fcl/narrowphase/detail/gjk_solver_libccd.h"
 #include "fcl/narrowphase/detail/conservative_advancement_func_matrix.h"
+#include "fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.h"
 
 namespace fcl
 {
diff --git a/include/fcl/narrowphase/detail/conservative_advancement_func_matrix.h b/include/fcl/narrowphase/detail/conservative_advancement_func_matrix.h
index 6f8fca537..91bd53483 100644
--- a/include/fcl/narrowphase/detail/conservative_advancement_func_matrix.h
+++ b/include/fcl/narrowphase/detail/conservative_advancement_func_matrix.h
@@ -51,7 +51,13 @@
 #include "fcl/object/geometry/shape/plane.h"
 #include "fcl/object/geometry/shape/sphere.h"
 #include "fcl/object/geometry/shape/triangle_p.h"
+#include "fcl/narrowphase/continuous_collision_request.h"
+#include "fcl/narrowphase/continuous_collision_result.h"
 #include "fcl/narrowphase/detail/traversal/traversal_recurse.h"
+#include "fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h"
+#include "fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h"
+#include "fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h"
+#include "fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h"
 
 namespace fcl
 {
@@ -64,7 +70,14 @@ struct ConservativeAdvancementFunctionMatrix
 {
   using S = typename NarrowPhaseSolver::S;
 
-  typedef S (*ConservativeAdvancementFunc)(const CollisionGeometry<S>* o1, const MotionBase<S>* motion1, const CollisionGeometry<S>* o2, const MotionBase<S>* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest<S>& request, ContinuousCollisionResult<S>& result);
+  using ConservativeAdvancementFunc
+      = S (*)(const CollisionGeometry<S>* o1,
+              const MotionBase<S>* motion1,
+              const CollisionGeometry<S>* o2,
+              const MotionBase<S>* motion2,
+              const NarrowPhaseSolver* nsolver,
+              const ContinuousCollisionRequest<S>& request,
+              ContinuousCollisionResult<S>& result);
 
   ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT];
 
diff --git a/include/fcl/object/geometry/shape/geometric_shapes_utility.h b/include/fcl/object/geometry/shape/geometric_shapes_utility.h
deleted file mode 100644
index 96420382b..000000000
--- a/include/fcl/object/geometry/shape/geometric_shapes_utility.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2011-2014, Willow Garage, Inc.
- *  Copyright (c) 2014-2016, Open Source Robotics Foundation
- *  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 Open Source Robotics Foundation 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 */
-
-#ifndef FCL_SHAPE_GEOMETRICSHAPESUTILITY_H
-#define FCL_SHAPE_GEOMETRICSHAPESUTILITY_H
-
-#warning "This header has been deprecated in FCL 0.6."
-
-#endif